diff --git a/src/ReferenceFrameRotations.jl b/src/ReferenceFrameRotations.jl index fde8464..cd866a8 100644 --- a/src/ReferenceFrameRotations.jl +++ b/src/ReferenceFrameRotations.jl @@ -52,6 +52,9 @@ include("dcm.jl") include("inv_rotations.jl") include("random.jl") include("quaternion.jl") +include("crp.jl") +include("mrp.jl") +include("shadow_rotation.jl") include("deprecations.jl") @@ -63,13 +66,25 @@ include("./conversions/angle_to_rot.jl") include("./conversions/angleaxis_to_angle.jl") include("./conversions/angleaxis_to_dcm.jl") include("./conversions/angleaxis_to_quat.jl") +include("./conversions/crp_to_angle.jl") +include("./conversions/crp_to_angleaxis.jl") +include("./conversions/crp_to_dcm.jl") +include("./conversions/crp_to_quat.jl") include("./conversions/api.jl") include("./conversions/dcm_to_angle.jl") include("./conversions/dcm_to_angleaxis.jl") +include("./conversions/dcm_to_crp.jl") +include("./conversions/dcm_to_mrp.jl") include("./conversions/dcm_to_quat.jl") +include("./conversions/mrp_to_angle.jl") +include("./conversions/mrp_to_angleaxis.jl") +include("./conversions/mrp_to_dcm.jl") +include("./conversions/mrp_to_quat.jl") include("./conversions/quat_to_angle.jl") include("./conversions/quat_to_angleaxis.jl") +include("./conversions/quat_to_crp.jl") include("./conversions/quat_to_dcm.jl") +include("./conversions/quat_to_mrp.jl") include("./conversions/smallangle_to_dcm.jl") include("./conversions/smallangle_to_quat.jl") include("./conversions/smallangle_to_rot.jl") diff --git a/src/compose_rotations.jl b/src/compose_rotations.jl index 816866a..dc8c298 100644 --- a/src/compose_rotations.jl +++ b/src/compose_rotations.jl @@ -90,6 +90,12 @@ end @inline compose_rotation(q::Quaternion) = q @inline compose_rotation(q::Quaternion, qs::Quaternion...) = q * compose_rotation(qs...) +@inline compose_rotation(c::CRP) = c +@inline compose_rotation(c::CRP, cs::CRP...) = compose_rotation(cs...) * c + +@inline compose_rotation(m::MRP) = m +@inline compose_rotation(m::MRP, ms::MRP...) = compose_rotation(ms...) * m + # This algorithm was proposed by @Per in # # https://discourse.julialang.org/t/improve-the-performance-of-multiplication-of-an-arbitrary-number-of-matrices/10835/24 diff --git a/src/conversions/api.jl b/src/conversions/api.jl index c0c0eb4..141face 100644 --- a/src/conversions/api.jl +++ b/src/conversions/api.jl @@ -9,6 +9,8 @@ Base.convert(::Type{<:DCM}, a::EulerAngles) = angle_to_dcm(a) Base.convert(::Type{<:DCM}, a::Quaternion) = quat_to_dcm(a) Base.convert(::Type{<:DCM}, a::EulerAngleAxis) = angleaxis_to_dcm(a) +Base.convert(::Type{<:DCM}, a::CRP) = crp_to_dcm(a) +Base.convert(::Type{<:DCM}, a::MRP) = mrp_to_dcm(a) # == Conversion to Euler Angles ============================================================ @@ -28,18 +30,48 @@ function Base.convert(::Type{<:_EulerAngleConversion{R}}, a::Quaternion) where R return quat_to_angle(a, R) end +function Base.convert(::Type{<:_EulerAngleConversion{R}}, a::CRP) where R + return crp_to_angle(a, R) +end + +function Base.convert(::Type{<:_EulerAngleConversion{R}}, a::MRP) where R + return mrp_to_angle(a, R) +end + Base.convert(::Type{<:EulerAngles}, a::DCM) = dcm_to_angle(a, :ZYX) Base.convert(::Type{<:EulerAngles}, a::EulerAngleAxis) = angleaxis_to_angle(a, :ZYX) Base.convert(::Type{<:EulerAngles}, a::Quaternion) = quat_to_angle(a, :ZYX) +Base.convert(::Type{<:EulerAngles}, a::CRP) = crp_to_angle(a, :ZYX) +Base.convert(::Type{<:EulerAngles}, a::MRP) = mrp_to_angle(a, :ZYX) # == Conversions to Euler Angle and Axis =================================================== Base.convert(::Type{<:EulerAngleAxis}, a::DCM) = dcm_to_angleaxis(a) Base.convert(::Type{<:EulerAngleAxis}, a::EulerAngles) = angle_to_angleaxis(a) Base.convert(::Type{<:EulerAngleAxis}, a::Quaternion) = quat_to_angleaxis(a) +Base.convert(::Type{<:EulerAngleAxis}, a::CRP) = crp_to_angleaxis(a) +Base.convert(::Type{<:EulerAngleAxis}, a::MRP) = mrp_to_angleaxis(a) # == Conversions to Quaternions ============================================================ Base.convert(::Type{<:Quaternion}, a::DCM) = dcm_to_quat(a) Base.convert(::Type{<:Quaternion}, a::EulerAngles) = angle_to_quat(a) Base.convert(::Type{<:Quaternion}, a::EulerAngleAxis) = angleaxis_to_quat(a) +Base.convert(::Type{<:Quaternion}, a::CRP) = crp_to_quat(a) +Base.convert(::Type{<:Quaternion}, a::MRP) = mrp_to_quat(a) + +# == Conversions to CRP ==================================================================== + +Base.convert(::Type{<:CRP}, a::DCM) = dcm_to_crp(a) +Base.convert(::Type{<:CRP}, a::Quaternion) = quat_to_crp(a) +Base.convert(::Type{<:CRP}, a::EulerAngles) = dcm_to_crp(angle_to_dcm(a)) +Base.convert(::Type{<:CRP}, a::EulerAngleAxis) = dcm_to_crp(angleaxis_to_dcm(a)) +Base.convert(::Type{<:CRP}, a::MRP) = dcm_to_crp(mrp_to_dcm(a)) + +# == Conversions to MRP ==================================================================== + +Base.convert(::Type{<:MRP}, a::DCM) = dcm_to_mrp(a) +Base.convert(::Type{<:MRP}, a::Quaternion) = quat_to_mrp(a) +Base.convert(::Type{<:MRP}, a::EulerAngles) = dcm_to_mrp(angle_to_dcm(a)) +Base.convert(::Type{<:MRP}, a::EulerAngleAxis) = dcm_to_mrp(angleaxis_to_dcm(a)) +Base.convert(::Type{<:MRP}, a::CRP) = dcm_to_mrp(crp_to_dcm(a)) diff --git a/src/conversions/crp_to_angle.jl b/src/conversions/crp_to_angle.jl new file mode 100644 index 0000000..7178286 --- /dev/null +++ b/src/conversions/crp_to_angle.jl @@ -0,0 +1,11 @@ +export crp_to_angle + +""" + crp_to_angle(c::CRP, rot_seq::Symbol) -> EulerAngles + +Convert CRP `c` to Euler Angles with rotation sequence `rot_seq`. +""" +function crp_to_angle(c::CRP, rot_seq::Symbol) + dcm = crp_to_dcm(c) + return dcm_to_angle(dcm, rot_seq) +end diff --git a/src/conversions/crp_to_angleaxis.jl b/src/conversions/crp_to_angleaxis.jl new file mode 100644 index 0000000..e992716 --- /dev/null +++ b/src/conversions/crp_to_angleaxis.jl @@ -0,0 +1,11 @@ +export crp_to_angleaxis + +""" + crp_to_angleaxis(c::CRP) -> EulerAngleAxis + +Convert CRP `c` to Euler Angle and Axis. +""" +function crp_to_angleaxis(c::CRP) + q = crp_to_quat(c) + return quat_to_angleaxis(q) +end diff --git a/src/conversions/crp_to_dcm.jl b/src/conversions/crp_to_dcm.jl new file mode 100644 index 0000000..caaee8d --- /dev/null +++ b/src/conversions/crp_to_dcm.jl @@ -0,0 +1,23 @@ +export crp_to_dcm + +""" + crp_to_dcm(c::CRP) -> DCM + +Convert CRP `c` to a Direction Cosine Matrix (DCM). +""" +function crp_to_dcm(c::CRP) + q = c + q_sq = q.q1^2 + q.q2^2 + q.q3^2 + denom = 1 + q_sq + + # Auxiliary variables to reduce computational burden. + q1q2 = q.q1 * q.q2 + q1q3 = q.q1 * q.q3 + q2q3 = q.q2 * q.q3 + + return DCM( + (1 + q.q1^2 - q.q2^2 - q.q3^2) / denom, 2 * (q1q2 + q.q3) / denom, 2 * (q1q3 - q.q2) / denom, + 2 * (q1q2 - q.q3) / denom, (1 - q.q1^2 + q.q2^2 - q.q3^2) / denom, 2 * (q2q3 + q.q1) / denom, + 2 * (q1q3 + q.q2) / denom, 2 * (q2q3 - q.q1) / denom, (1 - q.q1^2 - q.q2^2 + q.q3^2) / denom + )' +end diff --git a/src/conversions/crp_to_quat.jl b/src/conversions/crp_to_quat.jl new file mode 100644 index 0000000..6b5bc88 --- /dev/null +++ b/src/conversions/crp_to_quat.jl @@ -0,0 +1,13 @@ +export crp_to_quat + +""" + crp_to_quat(c::CRP) -> Quaternion + +Convert CRP `c` to a Quaternion. +""" +function crp_to_quat(c::CRP) + q_sq = c.q1^2 + c.q2^2 + c.q3^2 + β0 = 1 / sqrt(1 + q_sq) + + return Quaternion(β0, c.q1 * β0, c.q2 * β0, c.q3 * β0) +end diff --git a/src/conversions/dcm_to_crp.jl b/src/conversions/dcm_to_crp.jl new file mode 100644 index 0000000..4ea7440 --- /dev/null +++ b/src/conversions/dcm_to_crp.jl @@ -0,0 +1,14 @@ +export dcm_to_crp + +""" + dcm_to_crp(dcm::DCM) -> CRP + +Convert DCM `dcm` to CRP. +""" +function dcm_to_crp(dcm::DCM) + # Convert transformation matrix to quaternion. + q = dcm_to_quat(dcm) + + # Convert quaternion to CRP. + return quat_to_crp(q) +end diff --git a/src/conversions/dcm_to_mrp.jl b/src/conversions/dcm_to_mrp.jl new file mode 100644 index 0000000..b1e63f9 --- /dev/null +++ b/src/conversions/dcm_to_mrp.jl @@ -0,0 +1,10 @@ +export dcm_to_mrp + +""" + dcm_to_mrp(dcm::DCM) -> MRP + +Convert DCM `dcm` to MRP. +""" +function dcm_to_mrp(dcm::DCM) + return quat_to_mrp(dcm_to_quat(dcm)) +end diff --git a/src/conversions/mrp_to_angle.jl b/src/conversions/mrp_to_angle.jl new file mode 100644 index 0000000..e820f0d --- /dev/null +++ b/src/conversions/mrp_to_angle.jl @@ -0,0 +1,11 @@ +export mrp_to_angle + +""" + mrp_to_angle(m::MRP, rot_seq::Symbol) -> EulerAngles + +Convert MRP `m` to Euler Angles with rotation sequence `rot_seq`. +""" +function mrp_to_angle(m::MRP, rot_seq::Symbol) + dcm = mrp_to_dcm(m) + return dcm_to_angle(dcm, rot_seq) +end diff --git a/src/conversions/mrp_to_angleaxis.jl b/src/conversions/mrp_to_angleaxis.jl new file mode 100644 index 0000000..af203c3 --- /dev/null +++ b/src/conversions/mrp_to_angleaxis.jl @@ -0,0 +1,11 @@ +export mrp_to_angleaxis + +""" + mrp_to_angleaxis(m::MRP) -> EulerAngleAxis + +Convert MRP `m` to Euler Angle and Axis. +""" +function mrp_to_angleaxis(m::MRP) + q = mrp_to_quat(m) + return quat_to_angleaxis(q) +end diff --git a/src/conversions/mrp_to_dcm.jl b/src/conversions/mrp_to_dcm.jl new file mode 100644 index 0000000..c6e4879 --- /dev/null +++ b/src/conversions/mrp_to_dcm.jl @@ -0,0 +1,63 @@ +export mrp_to_dcm + +""" + mrp_to_dcm(m::MRP) -> DCM + +Convert MRP `m` to a Direction Cosine Matrix (DCM). +""" +function mrp_to_dcm(m::MRP) + # Direct formula from the provided image: + # [C] = [I] + (8 [skew(m)]^2 - 4(1 - |m|^2)[skew(m)]) / (1 + |m|^2)^2 + + s_sq = m.q1^2 + m.q2^2 + m.q3^2 + denom = (1 + s_sq)^2 + + fac1 = 8 / denom + fac2 = 4 * (1 - s_sq) / denom + + # Skew symmetric matrix components + # [ 0 -q3 q2] + # [ q3 0 -q1] + # [-q2 q1 0 ] + + # Precompute skew terms + sk_12 = -m.q3 + sk_13 = m.q2 + sk_21 = m.q3 + sk_23 = -m.q1 + sk_31 = -m.q2 + sk_32 = m.q1 + + # Skew^2 terms + # Row 1 + # sk2_11 = sk_12 * sk_21 + sk_13 * sk_31 = -q3*q3 + q2*(-q2) = -q3^2 - q2^2 + # sk2_12 = sk_13 * sk_32 = q2 * q1 + # sk2_13 = sk_12 * sk_23 = -q3 * -q1 = q1q3 + + sk2_11 = -m.q3^2 - m.q2^2 + sk2_12 = m.q1 * m.q2 + sk2_13 = m.q1 * m.q3 + + sk2_21 = sk2_12 + sk2_22 = -m.q3^2 - m.q1^2 + sk2_23 = m.q2 * m.q3 + + sk2_31 = sk2_13 + sk2_32 = sk2_23 + sk2_33 = -m.q2^2 - m.q1^2 + + # Combine + d11 = 1 + fac1 * sk2_11 + d12 = fac1 * sk2_12 - fac2 * sk_12 + d13 = fac1 * sk2_13 - fac2 * sk_13 + + d21 = fac1 * sk2_21 - fac2 * sk_21 + d22 = 1 + fac1 * sk2_22 + d23 = fac1 * sk2_23 - fac2 * sk_23 + + d31 = fac1 * sk2_31 - fac2 * sk_31 + d32 = fac1 * sk2_32 - fac2 * sk_32 + d33 = 1 + fac1 * sk2_33 + + return DCM(d11, d12, d13, d21, d22, d23, d31, d32, d33)' +end diff --git a/src/conversions/mrp_to_quat.jl b/src/conversions/mrp_to_quat.jl new file mode 100644 index 0000000..b4f3d1c --- /dev/null +++ b/src/conversions/mrp_to_quat.jl @@ -0,0 +1,16 @@ +export mrp_to_quat + +""" + mrp_to_quat(m::MRP) -> Quaternion + +Convert MRP `m` to a Quaternion. +""" +function mrp_to_quat(m::MRP) + s_sq = m.q1^2 + m.q2^2 + m.q3^2 + denom = 1 + s_sq + + β0 = (1 - s_sq) / denom + f = 2 / denom + + return Quaternion(β0, f * m.q1, f * m.q2, f * m.q3) +end diff --git a/src/conversions/quat_to_crp.jl b/src/conversions/quat_to_crp.jl new file mode 100644 index 0000000..cfe1f11 --- /dev/null +++ b/src/conversions/quat_to_crp.jl @@ -0,0 +1,14 @@ +export quat_to_crp + +""" + quat_to_crp(q::Quaternion) -> CRP + +Convert Quaternion `q` to CRP. +""" +function quat_to_crp(q::Quaternion) + if isapprox(q.q0, 0; atol = 1e-15) + throw(ArgumentError("The quaternion represents a rotation of 180 degrees, which is a singularity for CRP.")) + end + + return CRP(q.q1 / q.q0, q.q2 / q.q0, q.q3 / q.q0) +end diff --git a/src/conversions/quat_to_mrp.jl b/src/conversions/quat_to_mrp.jl new file mode 100644 index 0000000..8c670a0 --- /dev/null +++ b/src/conversions/quat_to_mrp.jl @@ -0,0 +1,24 @@ +export quat_to_mrp + +""" + quat_to_mrp(q::Quaternion) -> MRP + +Convert Quaternion `q` to MRP. +""" +function quat_to_mrp(q::Quaternion) + # Check for singularity: q0 = -1 (360 degrees rotation, which is 0 mod 360, but MRP singularity is at 360? + # Wait, MRP singularity is at +/- 360 degrees (q0 = -1). + # Normal MRP is singular at +/- 360 deg? + # No, MRP is singular at +/- 360 degrees (4*arctan(sigma)). + # sigma = tan(Phi/4). Phi = 360 -> tan(90) = inf. + # q0 = cos(Phi/2) = cos(180) = -1. + # So singularity is at q0 = -1. + + if isapprox(q.q0, -1; atol = 1e-15) + throw(ArgumentError("The quaternion represents a rotation of 360 degrees, which is a singularity for MRP.")) + end + + denom = 1 + q.q0 + + return MRP(q.q1 / denom, q.q2 / denom, q.q3 / denom) +end diff --git a/src/crp.jl b/src/crp.jl new file mode 100644 index 0000000..8fe7293 --- /dev/null +++ b/src/crp.jl @@ -0,0 +1,289 @@ +## Description ############################################################################# +# +# Functions related to the Classical Rodrigues Parameters (CRP). +# +############################################################################################ + +export dcrp + +############################################################################################ +# Constructors # +############################################################################################ + +""" + CRP(v::AbstractVector) + +Create a `CRP` from the vector `v`. +""" +function CRP(v::AbstractVector) + # The vector must have 3 components. + if length(v) != 3 + throw(ArgumentError("The input vector must have 3 components.")) + end + + return CRP(v[1], v[2], v[3]) +end + +CRP(I::UniformScaling) = CRP(0, 0, 0) + +############################################################################################ +# Julia API # +############################################################################################ + +# The following functions make sure that a CRP is an iterable object. This allows +# broadcasting without allocations. +Base.IndexStyle(::Type{<:CRP}) = IndexLinear() +Base.eltype(::Type{CRP{T}}) where T = T +Base.firstindex(c::CRP) = 1 +Base.lastindex(c::CRP) = 3 +Base.length(::CRP) = 3 +Base.ndims(::Type{<:CRP}) = 1 +Base.ndims(c::CRP) = 1 +Base.size(::CRP) = (3,) +Base.Broadcast.broadcastable(c::CRP) = c + +function Base.convert(::Type{CRP{T}}, c::CRP) where T + return CRP{T}(c.q1, c.q2, c.q3) +end + +@inline function Base.getindex(c::CRP, i::Int) + if i == 1 + return c.q1 + elseif i == 2 + return c.q2 + elseif i == 3 + return c.q3 + else + throw(BoundsError(c, i)) + end +end + +@inline function Base.getindex(c::CRP{T}, ::Colon) where T + return SVector{3, T}(c.q1, c.q2, c.q3) +end + +@inline function Base.getindex(c::CRP, i::CartesianIndex{1}) + return getindex(c, first(i.I)) +end + +@inline function Base.iterate(c::CRP) + return iterate(c, 0) +end + +@inline function Base.iterate(c::CRP, state::Integer) + state = state + 1 + + if state == 1 + return (c.q1, state) + elseif state == 2 + return (c.q2, state) + elseif state == 3 + return (c.q3, state) + else + return nothing + end +end + +# We need to define `setindex!` with respect to vectors to allow operations such as: +# +# v[4:6] = c +@inline function setindex!(v::Vector{T}, c::CRP, I::UnitRange) where T + # We can use all the functions in static arrays. + return setindex!(v, c[:], I) +end + +@inline function Base.:(==)(c1::CRP, c2::CRP) + return (c1.q1 == c2.q1) && + (c1.q2 == c2.q2) && + (c1.q3 == c2.q3) +end + +@inline function Base.isapprox(c1::CRP, c2::CRP; kwargs...) + return isapprox(c1.q1, c2.q1; kwargs...) && + isapprox(c1.q2, c2.q2; kwargs...) && + isapprox(c1.q3, c2.q3; kwargs...) +end + +############################################################################################ +# Kinematics # +############################################################################################ + +""" + dcrp(c::CRP, wba_b::AbstractArray) -> SVector{3} + +Compute the time-derivative of the CRP `c` that rotates a reference frame `a` into alignment +with the reference frame `b` in which the angular velocity of `b` with respect to `a`, and +represented in `b`, is `wba_b`. + +# Example + +```julia-repl +julia> c = CRP(0.0, 0.0, 0.0) + +julia> dcrp(c, [1.0, 0.0, 0.0]) +3-element StaticArrays.SVector{3, Float64} with indices SOneTo(3): + 0.5 + 0.0 + 0.0 +``` +""" +function dcrp(c::CRP, wba_b::AbstractArray) + # Check the dimensions. + if length(wba_b) != 3 + throw(ArgumentError("The angular velocity vector must have three components.")) + end + + # Auxiliary variables. + w = wba_b + + # Term 1: w + # (Just w) + + # Term 2: c x w + # c x w = [c2*w3 - c3*w2; c3*w1 - c1*w3; c1*w2 - c2*w1] + term2_1 = c.q2 * w[3] - c.q3 * w[2] + term2_2 = c.q3 * w[1] - c.q1 * w[3] + term2_3 = c.q1 * w[2] - c.q2 * w[1] + + # Term 3: (c . w) * c + c_dot_w = c.q1 * w[1] + c.q2 * w[2] + c.q3 * w[3] + term3 = c_dot_w * vect(c) + + return SVector{3}( + 0.5 * (w[1] + term2_1 + term3[1]), + 0.5 * (w[2] + term2_2 + term3[2]), + 0.5 * (w[3] + term2_3 + term3[3]) + ) +end + +############################################################################################ +# IO # +############################################################################################ + +function Base.show(io::IO, c::CRP{T}) where T + # Check if the user wants compact printing, defaulting to `true`. + compact_printing = get(io, :compact, true)::Bool + + print(io, "CRP{$(T)}: ") + print(io, c.q1, ", ", c.q2, ", ", c.q3) + + return nothing +end + +function Base.show(io::IO, mime::MIME"text/plain", c::CRP{T}) where T + # Check if the user wants colors. + color = get(io, :color, false)::Bool + + # Check if the user wants compact printing, defaulting to `true`. + compact_printing = get(io, :compact, true)::Bool + + # Assemble the context. + context = IOContext(io, :color => color, :compact => compact_printing) + + print(io, "CRP{$(T)}: ") + print(context, c.q1, ", ", c.q2, ", ", c.q3) +end + +############################################################################################ +# Operations # +############################################################################################ + +# == Operation: + ========================================================================== + +@inline +(c1::CRP, c2::CRP) = CRP(c1.q1 + c2.q1, c1.q2 + c2.q2, c1.q3 + c2.q3) + +# == Operation: - ========================================================================== + +@inline -(c::CRP) = CRP(-c.q1, -c.q2, -c.q3) +@inline -(c1::CRP, c2::CRP) = CRP(c1.q1 - c2.q1, c1.q2 - c2.q2, c1.q3 - c2.q3) + +# == Operation: * ========================================================================== + +@inline *(λ::Number, c::CRP) = CRP(λ * c.q1, λ * c.q2, λ * c.q3) +@inline *(c::CRP, λ::Number) = CRP(c.q1 * λ, c.q2 * λ, c.q3 * λ) + +""" + *(c1::CRP, c2::CRP) -> CRP + +Compute the composition of two CRPs `c1` and `c2`. + + C3 = C2 * C1 + +which means that `C3` acts as `C1` followed by `C2`. +""" +function Base.:*(c1::CRP, c2::CRP) + norm_c1_c2 = c1.q1 * c2.q1 + c1.q2 * c2.q2 + c1.q3 * c2.q3 + + if isapprox(norm_c1_c2, 1; atol = 1e-15) + # TODO: Return a specific error? + throw(ArgumentError("The composition of these CRPs results in a specific singularity (180 degrees rotation).")) + end + + denom = 1 - norm_c1_c2 + + return CRP( + (c1.q1 + c2.q1 - c1.q2 * c2.q3 + c1.q3 * c2.q2) / denom, + (c1.q2 + c2.q2 - c1.q3 * c2.q1 + c1.q1 * c2.q3) / denom, + (c1.q3 + c2.q3 - c1.q1 * c2.q2 + c1.q2 * c2.q1) / denom + ) +end + +# == Operation: / ========================================================================== + +@inline /(c::CRP, λ::Number) = CRP(c.q1 / λ, c.q2 / λ, c.q3 / λ) +@inline /(c1::CRP, c2::CRP) = c1 * inv(c2) + +# == Operation: \ ========================================================================== + +@inline \(c1::CRP, c2::CRP) = inv(c1) * c2 + +############################################################################################ +# Functions # +############################################################################################ + +""" + inv(c::CRP) -> CRP + +Compute the inverse of the CRP `c`. +""" +@inline inv(c::CRP) = -c + +""" + norm(c::CRP) -> Number + +Compute the Euclidean norm of the CRP `c`. +""" +@inline norm(c::CRP) = √(c.q1^2 + c.q2^2 + c.q3^2) + +@inline one(::Type{CRP{T}}) where T = CRP{T}(T(0), T(0), T(0)) +@inline one(::Type{CRP}) = CRP{Float64}(0, 0, 0) +@inline one(c::CRP{T}) where T = one(CRP{T}) + +@inline zero(::Type{CRP{T}}) where T = CRP{T}(T(0), T(0), T(0)) +@inline zero(::Type{CRP}) = CRP{Float64}(0, 0, 0) +@inline zero(c::CRP{T}) where T = zero(CRP{T}) + +""" + copy(c::CRP) -> CRP + +Create a copy of the CRP `c`. +""" +@inline copy(c::CRP{T}) where T = CRP{T}(c.q1, c.q2, c.q3) + +""" + vect(c::CRP) -> SVector{3, T} + +Return the vector definition of the CRP `c`: + + [q1, q2, q3] +""" +@inline vect(c::CRP) = SVector{3}(c.q1, c.q2, c.q3) + +# == UniformScaling ======================================================================== + +@inline *(u::UniformScaling, c::CRP) = CRP(u) * c +@inline *(c::CRP, u::UniformScaling) = c * CRP(u) +@inline /(u::UniformScaling, c::CRP) = CRP(u) / c +@inline /(c::CRP, u::UniformScaling) = c / CRP(u) +@inline \(u::UniformScaling, c::CRP) = CRP(u) \ c +@inline \(c::CRP, u::UniformScaling) = c \ CRP(u) diff --git a/src/inv_rotations.jl b/src/inv_rotations.jl index 01fd28d..f054917 100644 --- a/src/inv_rotations.jl +++ b/src/inv_rotations.jl @@ -17,8 +17,10 @@ Compute the inverse rotation of `R`, which can be: - A direction cosine matrix (`DCM`); - An Euler angle and axis (`EulerAngleAxis`); -- A set of Euler anlges (`EulerAngles`); or -- A quaternion (`Quaternion`). +- A set of Euler anlges (`EulerAngles`); +- A quaternion (`Quaternion`); +- A classical Rodrigues parameter (`CRP`); or +- A modified Rodrigues parameter (`MRP`). The output will have the same type as `R`. @@ -71,3 +73,5 @@ Quaternion{Float64}: @inline inv_rotation(ea::EulerAngleAxis) = inv(ea) @inline inv_rotation(Θ::EulerAngles) = inv(Θ) @inline inv_rotation(q::Quaternion) = conj(q) +@inline inv_rotation(q::CRP) = inv(q) +@inline inv_rotation(s::MRP) = inv(s) diff --git a/src/mrp.jl b/src/mrp.jl new file mode 100644 index 0000000..bf1e7a0 --- /dev/null +++ b/src/mrp.jl @@ -0,0 +1,301 @@ +## Description ############################################################################# +# +# Functions related to the Modified Rodrigues Parameters (MRP). +# +############################################################################################ + +export dmrp + +############################################################################################ +# Constructors # +############################################################################################ + +""" + MRP(v::AbstractVector) + +Create a `MRP` from the vector `v`. +""" +function MRP(v::AbstractVector) + # The vector must have 3 components. + if length(v) != 3 + throw(ArgumentError("The input vector must have 3 components.")) + end + + return MRP(v[1], v[2], v[3]) +end + +MRP(I::UniformScaling) = MRP(0, 0, 0) + +############################################################################################ +# Julia API # +############################################################################################ + +# The following functions make sure that a MRP is an iterable object. This allows +# broadcasting without allocations. +Base.IndexStyle(::Type{<:MRP}) = IndexLinear() +Base.eltype(::Type{MRP{T}}) where T = T +Base.firstindex(m::MRP) = 1 +Base.lastindex(m::MRP) = 3 +Base.length(::MRP) = 3 +Base.ndims(::Type{<:MRP}) = 1 +Base.ndims(m::MRP) = 1 +Base.size(::MRP) = (3,) +Base.Broadcast.broadcastable(m::MRP) = m + +function Base.convert(::Type{MRP{T}}, m::MRP) where T + return MRP{T}(m.q1, m.q2, m.q3) +end + +@inline function Base.getindex(m::MRP, i::Int) + if i == 1 + return m.q1 + elseif i == 2 + return m.q2 + elseif i == 3 + return m.q3 + else + throw(BoundsError(m, i)) + end +end + +@inline function Base.getindex(m::MRP{T}, ::Colon) where T + return SVector{3, T}(m.q1, m.q2, m.q3) +end + +@inline function Base.getindex(m::MRP, i::CartesianIndex{1}) + return getindex(m, first(i.I)) +end + +@inline function Base.iterate(m::MRP) + return iterate(m, 0) +end + +@inline function Base.iterate(m::MRP, state::Integer) + state = state + 1 + + if state == 1 + return (m.q1, state) + elseif state == 2 + return (m.q2, state) + elseif state == 3 + return (m.q3, state) + else + return nothing + end +end + +# We need to define `setindex!` with respect to vectors to allow operations such as: +# +# v[4:6] = m +@inline function setindex!(v::Vector{T}, m::MRP, I::UnitRange) where T + # We can use all the functions in static arrays. + return setindex!(v, m[:], I) +end + +@inline function Base.:(==)(m1::MRP, m2::MRP) + return (m1.q1 == m2.q1) && + (m1.q2 == m2.q2) && + (m1.q3 == m2.q3) +end + +@inline function Base.isapprox(m1::MRP, m2::MRP; kwargs...) + return isapprox(m1.q1, m2.q1; kwargs...) && + isapprox(m1.q2, m2.q2; kwargs...) && + isapprox(m1.q3, m2.q3; kwargs...) +end + +############################################################################################ +# Kinematics # +############################################################################################ + +""" + dmrp(m::MRP, wba_b::AbstractArray) -> SVector{3} + +Compute the time-derivative of the MRP `m` that rotates a reference frame `a` into alignment +with the reference frame `b` in which the angular velocity of `b` with respect to `a`, and +represented in `b`, is `wba_b`. + +# Example + +```julia-repl +julia> m = MRP(0.0, 0.0, 0.0) + +julia> dmrp(m, [1.0, 0.0, 0.0]) +3-element StaticArrays.SVector{3, Float64} with indices SOneTo(3): + 0.25 + 0.0 + 0.0 +``` +""" +function dmrp(m::MRP, wba_b::AbstractArray) + # Check the dimensions. + if length(wba_b) != 3 + throw(ArgumentError("The angular velocity vector must have three components.")) + end + + # Auxiliary variables. + s2 = m.q1^2 + m.q2^2 + m.q3^2 + w = wba_b + + # Term 1: (1 - s^2) * w + term1 = (1 - s2) * w + + # Term 2: 2 * (s x w) + # s x w = [s2*w3 - s3*w2; s3*w1 - s1*w3; s1*w2 - s2*w1] + # We use the explicit cross product for speed. + term2_1 = 2 * (m.q2 * w[3] - m.q3 * w[2]) + term2_2 = 2 * (m.q3 * w[1] - m.q1 * w[3]) + term2_3 = 2 * (m.q1 * w[2] - m.q2 * w[1]) + + # Term 3: 2 * (s . w) * s + s_dot_w = m.q1 * w[1] + m.q2 * w[2] + m.q3 * w[3] + term3 = 2 * s_dot_w * vect(m) + + return SVector{3}( + 0.25 * (term1[1] + term2_1 + term3[1]), + 0.25 * (term1[2] + term2_2 + term3[2]), + 0.25 * (term1[3] + term2_3 + term3[3]) + ) +end + +############################################################################################ +# IO # +############################################################################################ + +function Base.show(io::IO, m::MRP{T}) where T + # Check if the user wants compact printing, defaulting to `true`. + compact_printing = get(io, :compact, true)::Bool + + print(io, "MRP{$(T)}: ") + print(io, m.q1, ", ", m.q2, ", ", m.q3) + + return nothing +end + +function Base.show(io::IO, mime::MIME"text/plain", m::MRP{T}) where T + # Check if the user wants colors. + color = get(io, :color, false)::Bool + + # Check if the user wants compact printing, defaulting to `true`. + compact_printing = get(io, :compact, true)::Bool + + # Assemble the context. + context = IOContext(io, :color => color, :compact => compact_printing) + + print(io, "MRP{$(T)}: ") + print(io, m.q1, ", ", m.q2, ", ", m.q3) +end + +############################################################################################ +# Operations # +############################################################################################ + +# == Operation: + ========================================================================== + +@inline +(m1::MRP, m2::MRP) = MRP(m1.q1 + m2.q1, m1.q2 + m2.q2, m1.q3 + m2.q3) + +# == Operation: - ========================================================================== + +@inline -(m::MRP) = MRP(-m.q1, -m.q2, -m.q3) +@inline -(m1::MRP, m2::MRP) = MRP(m1.q1 - m2.q1, m1.q2 - m2.q2, m1.q3 - m2.q3) + +# == Operation: * ========================================================================== + +@inline *(λ::Number, m::MRP) = MRP(λ * m.q1, λ * m.q2, λ * m.q3) +@inline *(m::MRP, λ::Number) = MRP(m.q1 * λ, m.q2 * λ, m.q3 * λ) + +""" + *(m1::MRP, m2::MRP) -> MRP + +Compute the composition of two MRPs `m1` and `m2`. + + M3 = M2 * M1 + +which means that `M3` acts as `M1` followed by `M2`. +""" +function Base.:*(m1::MRP, m2::MRP) + # Direct formula for MRP composition (Attitude Addition). + # [FN(m)] = [FB(m2)][BN(m1)] + # m = ((1 - |m1|^2)m2 + (1 - |m2|^2)m1 - 2(m2 x m1)) / (1 + |m1|^2|m2|^2 - 2(m1 . m2)) + + s1_sq = m1.q1^2 + m1.q2^2 + m1.q3^2 + s2_sq = m2.q1^2 + m2.q2^2 + m2.q3^2 + + dot_prod = m1.q1 * m2.q1 + m1.q2 * m2.q2 + m1.q3 * m2.q3 + + denom = 1 + s1_sq * s2_sq - 2 * dot_prod + + # Using cross product inline for performance + cross_1 = m2.q2 * m1.q3 - m2.q3 * m1.q2 + cross_2 = m2.q3 * m1.q1 - m2.q1 * m1.q3 + cross_3 = m2.q1 * m1.q2 - m2.q2 * m1.q1 + + term1_fac = 1 - s1_sq + term2_fac = 1 - s2_sq + + q1 = (term1_fac * m2.q1 + term2_fac * m1.q1 + 2 * cross_1) / denom + q2 = (term1_fac * m2.q2 + term2_fac * m1.q2 + 2 * cross_2) / denom + q3 = (term1_fac * m2.q3 + term2_fac * m1.q3 + 2 * cross_3) / denom + + return MRP(q1, q2, q3) +end + +# == Operation: / ========================================================================== + +@inline /(m::MRP, λ::Number) = MRP(m.q1 / λ, m.q2 / λ, m.q3 / λ) +@inline /(m1::MRP, m2::MRP) = m1 * inv(m2) + +# == Operation: \ ========================================================================== + +@inline \(m1::MRP, m2::MRP) = inv(m1) * m2 + +############################################################################################ +# Functions # +############################################################################################ + +""" + inv(m::MRP) -> MRP + +Compute the inverse of the MRP `m`. +""" +@inline inv(m::MRP) = -m + +""" + norm(m::MRP) -> Number + +Compute the Euclidean norm of the MRP `m`. +""" +@inline norm(m::MRP) = √(m.q1^2 + m.q2^2 + m.q3^2) + +@inline one(::Type{MRP{T}}) where T = MRP{T}(T(0), T(0), T(0)) +@inline one(::Type{MRP}) = MRP{Float64}(0, 0, 0) +@inline one(m::MRP{T}) where T = one(MRP{T}) + +@inline zero(::Type{MRP{T}}) where T = MRP{T}(T(0), T(0), T(0)) +@inline zero(::Type{MRP}) = MRP{Float64}(0, 0, 0) +@inline zero(m::MRP{T}) where T = zero(MRP{T}) + +""" + copy(m::MRP) -> MRP + +Create a copy of the MRP `m`. +""" +@inline copy(m::MRP{T}) where T = MRP{T}(m.q1, m.q2, m.q3) + +""" + vect(m::MRP) -> SVector{3, T} + +Return the vector definition of the MRP `m`: + + [q1, q2, q3] +""" +@inline vect(m::MRP) = SVector{3}(m.q1, m.q2, m.q3) + +# == UniformScaling ======================================================================== + +@inline *(u::UniformScaling, m::MRP) = MRP(u) * m +@inline *(m::MRP, u::UniformScaling) = m * MRP(u) +@inline /(u::UniformScaling, m::MRP) = MRP(u) / m +@inline /(m::MRP, u::UniformScaling) = m / MRP(u) +@inline \(u::UniformScaling, m::MRP) = MRP(u) \ m +@inline \(m::MRP, u::UniformScaling) = m \ MRP(u) diff --git a/src/random.jl b/src/random.jl index bc14f55..c616494 100644 --- a/src/random.jl +++ b/src/random.jl @@ -63,6 +63,28 @@ function Random.rand(rng::AbstractRNG, ::Random.SamplerType{R}) where R <: Quate return _rand_quat(rng, T) end +# == CRP =================================================================================== + +function Random.rand(rng::AbstractRNG, ::Random.SamplerType{R}) where R <: CRP + T = eltype(R) + if T == Any + T = Float64 + end + + return quat_to_crp(_rand_quat(rng, T)) +end + +# == MRP =================================================================================== + +function Random.rand(rng::AbstractRNG, ::Random.SamplerType{R}) where R <: MRP + T = eltype(R) + if T == Any + T = Float64 + end + + return quat_to_mrp(_rand_quat(rng, T)) +end + ############################################################################################ # Private Functions # ############################################################################################ diff --git a/src/shadow_rotation.jl b/src/shadow_rotation.jl new file mode 100644 index 0000000..471f38f --- /dev/null +++ b/src/shadow_rotation.jl @@ -0,0 +1,29 @@ +## Description ############################################################################# +# +# Functions to compute the shadow rotation. +# +############################################################################################ + +export shadow_rotation + +""" + shadow_rotation(c::CRP) -> CRP + +Compute the shadow rotation of the CRP `c`. + +The shadow rotation of a CRP is the rotation itself: `c`. +""" +@inline shadow_rotation(c::CRP) = c + +""" + shadow_rotation(m::MRP) -> MRP + +Compute the shadow rotation of the MRP `m`. + +The shadow rotation of a MRP `m` is formed by the values `q` such that: + + |q| > 1 + +and represents the same rotation as `m`. +""" +@inline shadow_rotation(m::MRP) = -m / (norm(m)^2) \ No newline at end of file diff --git a/src/types.jl b/src/types.jl index c538948..01adca8 100644 --- a/src/types.jl +++ b/src/types.jl @@ -4,7 +4,7 @@ # ############################################################################################ -export DCM, EulerAngles, EulerAngleAxis, Quaternion, ReferenceFrameRotation +export DCM, EulerAngles, EulerAngleAxis, Quaternion, CRP, MRP, ReferenceFrameRotation """ DCM{T} @@ -198,6 +198,62 @@ struct Quaternion{T} q3::T end +""" + CRP{T} + +The definition of Classical Rodrigues Parameters (CRP). + +# Fields + +- `q1::T`: First component of the CRP. +- `q2::T`: Second component of the CRP. +- `q3::T`: Third component of the CRP. + +# Constructor + + CRP(q1::T1, q2::T2, q3::T3) where {T1, T2, T3} + +Create a new instance of `CRP` with coordinates `q1`, `q2`, and `q3`. +""" +struct CRP{T} + q1::T + q2::T + q3::T +end + +function CRP(q1::T1, q2::T2, q3::T3) where {T1, T2, T3} + T = promote_type(T1, T2, T3) + return CRP{T}(T(q1), T(q2), T(q3)) +end + +""" + MRP{T} + +The definition of Modified Rodrigues Parameters (MRP). + +# Fields + +- `q1::T`: First component of the MRP. +- `q2::T`: Second component of the MRP. +- `q3::T`: Third component of the MRP. + +# Constructor + + MRP(q1::T1, q2::T2, q3::T3) where {T1, T2, T3} + +Create a new instance of `MRP` with coordinates `q1`, `q2`, and `q3`. +""" +struct MRP{T} + q1::T + q2::T + q3::T +end + +function MRP(q1::T1, q2::T2, q3::T3) where {T1, T2, T3} + T = promote_type(T1, T2, T3) + return MRP{T}(T(q1), T(q2), T(q3)) +end + """ ReferenceFramerotation @@ -207,6 +263,8 @@ const ReferenceFrameRotation = Union{ DCM, EulerAngles, EulerAngleAxis, - Quaternion + Quaternion, + CRP, + MRP } diff --git a/test/crp.jl b/test/crp.jl new file mode 100644 index 0000000..43978fe --- /dev/null +++ b/test/crp.jl @@ -0,0 +1,251 @@ +@testset "CRP Constructors" begin + c = CRP(1.0, 2.0, 3.0) + @test c.q1 == 1.0 + @test c.q2 == 2.0 + @test c.q3 == 3.0 + @test eltype(c) == Float64 + + c = CRP(1, 2, 3) + @test c.q1 == 1 + @test c.q2 == 2 + @test c.q3 == 3 + @test eltype(c) == Int + + c = CRP([1.0, 2.0, 3.0]) + @test c.q1 == 1.0 + @test c.q2 == 2.0 + @test c.q3 == 3.0 + + c = CRP(I) + @test c.q1 == 0 + @test c.q2 == 0 + @test c.q3 == 0 + + # Test indexing and iteration + @test c[1] == 0 + @test c[2] == 0 + @test c[3] == 0 + @test collect(c) == [0, 0, 0] + @test length(c) == 3 +end + +@testset "CRP Show" begin + c = CRP(1.0, 2.0, 3.0) + io = IOBuffer() + show(io, c) + s = String(take!(io)) + @test occursin("CRP{Float64}", s) + @test occursin("1.0", s) + @test occursin("2.0", s) + @test occursin("3.0", s) +end + +@testset "CRP Conversions" begin + # Helper to compare DCMs + function isapprox_dcm(d1, d2; atol = 1e-12) + return maximum(abs.(d1 - d2)) < atol + end + + # Random rotation + eul = EulerAngles(1.0, 0.5, -0.2, :ZYX) + dcm = angle_to_dcm(eul) + q = angle_to_quat(eul) + + # DCM -> Quaternion -> CRP + c = dcm_to_crp(dcm) + + # CRP -> DCM + dcm_c = crp_to_dcm(c) + @test isapprox_dcm(dcm, dcm_c) + + # CRP -> Quaternion + q_c = crp_to_quat(c) + # Signs of quaternions can be flipped, so check if they Are approximately equal or opposite + @test isapprox(q, q_c; atol=1e-12) || isapprox(q, -q_c; atol=1e-12) + + # Quaternion -> CRP + c_q = quat_to_crp(q) + @test isapprox(c, c_q; atol=1e-12) + + # CRP -> EulerAngles + ang_c = crp_to_angle(c, :ZYX) + @test isapprox(ang_c.a1, eul.a1; atol=1e-12) + @test isapprox(ang_c.a2, eul.a2; atol=1e-12) + @test isapprox(ang_c.a3, eul.a3; atol=1e-12) + + # CRP -> EulerAngleAxis + eaa_c = crp_to_angleaxis(c) + eaa = angle_to_angleaxis(eul) + @test isapprox(eaa_c.a, eaa.a; atol=1e-12) + @test isapprox(eaa_c.v, eaa.v; atol=1e-12) + + # 180 degree rotation singularity check + q_180 = Quaternion(0.0, 1.0, 0.0, 0.0) # 180 deg about X + @test_throws ArgumentError quat_to_crp(q_180) +end + +@testset "CRP Composition" begin + # c3 = c2 * c1 (apply c1 then c2) -> R3 = R2 * R1 + + eul1 = EulerAngles(0.3, 0.2, 0.1, :ZYX) + eul2 = EulerAngles(-0.2, 0.4, -0.3, :ZYX) + + c1 = dcm_to_crp(angle_to_dcm(eul1)) + c2 = dcm_to_crp(angle_to_dcm(eul2)) + + c3 = c2 * c1 + + # Verify with DCM + dcm1 = angle_to_dcm(eul1) + dcm2 = angle_to_dcm(eul2) + dcm3 = dcm2 * dcm1 + + dcm_c3 = crp_to_dcm(c3) + @test maximum(abs.(dcm3 - dcm_c3)) < 1e-12 + + # Verify compose_rotation + c_comp = compose_rotation(c1, c2) # Apply c1 then c2 + @test isapprox(c_comp, c3; atol=1e-12) +end + +@testset "CRP Arithmetic" begin + c1 = CRP(1.0, 2.0, 3.0) + c2 = CRP(0.5, -0.5, 1.0) + + # + + c3 = c1 + c2 + @test isapprox(c3.q1, 1.5; atol=1e-12) + @test isapprox(c3.q2, 1.5; atol=1e-12) + @test isapprox(c3.q3, 4.0; atol=1e-12) + + # - (binary) + c4 = c1 - c2 + @test c4.q1 == 0.5 + @test c4.q2 == 2.5 + @test c4.q3 == 2.0 + + # - (unary) + c_neg = -c1 + @test c_neg.q1 == -1.0 + @test c_neg.q2 == -2.0 + @test c_neg.q3 == -3.0 + + # * (scalar) + c_scaled = 2.0 * c1 + @test c_scaled.q1 == 2.0 + @test c_scaled.q2 == 4.0 + @test c_scaled.q3 == 6.0 + + c_scaled2 = c1 * 2.0 + @test c_scaled2 == c_scaled + + # / (scalar) + c_div = c1 / 2.0 + @test c_div.q1 == 0.5 + @test c_div.q2 == 1.0 + @test c_div.q3 == 1.5 +end + +@testset "CRP Inverse and Division" begin + c1 = CRP(1.0, 2.0, 3.0) + c2 = CRP(0.5, -0.5, 1.0) + + # inv + c_inv = inv(c1) + # The inverse of a CRP q is -q + @test c_inv == -c1 + + # Identity + # c * inv(c) should be identity (0, 0, 0) + c_identity = c1 * c_inv + @test isapprox(c_identity.q1, 0.0; atol=1e-12) + @test isapprox(c_identity.q2, 0.0; atol=1e-12) + @test isapprox(c_identity.q3, 0.0; atol=1e-12) + + # / (c1 / c2 = c1 * inv(c2)) + c_div = c1 / c2 + c_mult = c1 * inv(c2) + @test isapprox(c_div, c_mult) + + # \ (c1 \ c2 = inv(c1) * c2) + c_backdiv = c1 \ c2 + c_mult_back = inv(c1) * c2 + @test isapprox(c_backdiv, c_mult_back) +end + +@testset "CRP Utils" begin + c = CRP(3.0, 0.0, 4.0) + @test norm(c) == 5.0 + + @test one(CRP) == CRP(0.0, 0.0, 0.0) + @test one(c) == CRP(0.0, 0.0, 0.0) + + @test zero(CRP) == CRP(0.0, 0.0, 0.0) + @test zero(c) == CRP(0.0, 0.0, 0.0) + + # copy + c_copy = copy(c) + @test c_copy == c + + # vect + v = vect(c) + @test v isa SVector{3, Float64} + @test v[1] == c.q1 + @test v[2] == c.q2 + @test v[3] == c.q3 + + # UniformScaling + @test I * c == c + @test c * I == c + @test I / c == inv(c) + @test c / I == c + @test I \ c == c + @test c \ I == inv(c) +end + +@testset "CRP Shadow Rotation" begin + c = CRP(1.0, 2.0, 3.0) + c_shadow = shadow_rotation(c) + + @test c == c_shadow +end + +@testset "CRP Random" begin + Random.seed!(123) + c = rand(CRP) + @test c isa CRP{Float64} + + c_float32 = rand(CRP{Float32}) + @test c_float32 isa CRP{Float32} +end + +@testset "CRP Kinematics" begin + # Test analytical derivative against numerical derivative + for T in (Float64,) + # We need a seed to ensure reproducibility + Random.seed!(123) + + c = rand(CRP{T}) + w = @SVector randn(T, 3) + + dc = dcrp(c, w) + + dt = 1e-8 + + # Use DCM to propagate + D = crp_to_dcm(c) + dD = ddcm(D, w) + D_next = D + dD * dt + D_next = orthonormalize(D_next) + + c_next = dcm_to_crp(D_next) + + dc_num = (c_next - c) / dt + + # Check alignment + @test isapprox(dc, dc_num[:]; rtol = 1e-4, atol=1e-6) + end +end + + + diff --git a/test/mrp.jl b/test/mrp.jl new file mode 100644 index 0000000..beb98c6 --- /dev/null +++ b/test/mrp.jl @@ -0,0 +1,274 @@ +@testset "MRP Constructors" begin + m = MRP(1.0, 2.0, 3.0) + @test m.q1 == 1.0 + @test m.q2 == 2.0 + @test m.q3 == 3.0 + @test eltype(m) == Float64 + + m = MRP(1, 2, 3) + @test m.q1 == 1 + @test m.q2 == 2 + @test m.q3 == 3 + @test eltype(m) == Int + + m = MRP([1.0, 2.0, 3.0]) + @test m.q1 == 1.0 + @test m.q2 == 2.0 + @test m.q3 == 3.0 + + m = MRP(I) + @test m.q1 == 0 + @test m.q2 == 0 + @test m.q3 == 0 + + # Test indexing and iteration + @test m[1] == 0 + @test m[2] == 0 + @test m[3] == 0 + @test collect(m) == [0, 0, 0] + @test length(m) == 3 +end + +@testset "MRP Show" begin + m = MRP(1.0, 2.0, 3.0) + io = IOBuffer() + show(io, m) + s = String(take!(io)) + @test occursin("MRP{Float64}", s) + @test occursin("1.0", s) + @test occursin("2.0", s) + @test occursin("3.0", s) +end + +@testset "MRP Conversions" begin + # Helper to compare DCMs + function isapprox_dcm(d1, d2; atol = 1e-12) + return maximum(abs.(d1 - d2)) < atol + end + + # Random rotation + eul = EulerAngles(1.0, 0.5, -0.2, :ZYX) + dcm = angle_to_dcm(eul) + q = angle_to_quat(eul) + + # DCM -> MRP + m = dcm_to_mrp(dcm) + + # MRP -> DCM + dcm_m = mrp_to_dcm(m) + @test isapprox_dcm(dcm, dcm_m) + + # MRP -> Quaternion + q_m = mrp_to_quat(m) + # Signs of quaternions can be flipped, so check if they Are approximately equal or opposite + @test isapprox(q, q_m; atol=1e-12) || isapprox(q, -q_m; atol=1e-12) + + # Quaternion -> MRP + m_q = quat_to_mrp(q) + @test isapprox(m, m_q; atol=1e-12) + + # MRP -> EulerAngles + ang_m = mrp_to_angle(m, :ZYX) + @test isapprox(ang_m.a1, eul.a1; atol=1e-12) + @test isapprox(ang_m.a2, eul.a2; atol=1e-12) + @test isapprox(ang_m.a3, eul.a3; atol=1e-12) + + # MRP -> EulerAngleAxis + eaa_m = mrp_to_angleaxis(m) + eaa = angle_to_angleaxis(eul) + @test isapprox(eaa_m.a, eaa.a; atol=1e-12) + @test isapprox(eaa_m.v, eaa.v; atol=1e-12) + + # 360 degree rotation singularity check (MRP singular at 360) + q_360 = Quaternion(-1.0, 0.0, 0.0, 0.0) # 360 deg is q0 = -1 (since angle is 2*acos(q0), q0=-1 => angle=360) + @test_throws ArgumentError quat_to_mrp(q_360) +end + +@testset "MRP Composition" begin + # m3 = m2 * m1 + + eul1 = EulerAngles(0.3, 0.2, 0.1, :ZYX) + eul2 = EulerAngles(-0.2, 0.4, -0.3, :ZYX) + + m1 = dcm_to_mrp(angle_to_dcm(eul1)) + m2 = dcm_to_mrp(angle_to_dcm(eul2)) + + m3 = m2 * m1 + + # Verify with DCM + dcm1 = angle_to_dcm(eul1) + dcm2 = angle_to_dcm(eul2) + dcm3 = dcm2 * dcm1 + + dcm_m3 = mrp_to_dcm(m3) + @test maximum(abs.(dcm3 - dcm_m3)) < 1e-12 + + # Verify compose_rotation + m_comp = compose_rotation(m1, m2) # Apply m1 then m2 + @test isapprox(m_comp, m3; atol=1e-12) +end + +@testset "MRP Arithmetic" begin + m1 = MRP(0.1, 0.2, 0.3) + m2 = MRP(0.05, -0.05, 0.1) + + # + + m3 = m1 + m2 + @test isapprox(m3.q1, 0.15; atol=1e-12) + @test isapprox(m3.q2, 0.15; atol=1e-12) + @test isapprox(m3.q3, 0.4; atol=1e-12) + + # - (binary) + m4 = m1 - m2 + @test isapprox(m4.q1, 0.05; atol=1e-12) + @test isapprox(m4.q2, 0.25; atol=1e-12) + @test isapprox(m4.q3, 0.2; atol=1e-12) + + # - (unary) + m_neg = -m1 + @test m_neg.q1 == -0.1 + @test m_neg.q2 == -0.2 + @test m_neg.q3 == -0.3 + + # * (scalar) + m_scaled = 2.0 * m1 + @test m_scaled.q1 == 0.2 + @test m_scaled.q2 == 0.4 + @test m_scaled.q3 == 0.6 + + m_scaled2 = m1 * 2.0 + @test m_scaled2 == m_scaled + + # / (scalar) + m_div = m1 / 2.0 + @test m_div.q1 == 0.05 + @test m_div.q2 == 0.1 + @test m_div.q3 == 0.15 +end + +@testset "MRP Inverse and Division" begin + m1 = MRP(0.1, 0.2, 0.3) + m2 = MRP(0.05, -0.05, 0.1) + + # inv + m_inv = inv(m1) + # The inverse of a MRP q is -q + @test m_inv == -m1 + + # Identity + # m * inv(m) should be identity (0, 0, 0) + m_identity = m1 * m_inv + @test isapprox(m_identity.q1, 0.0; atol=1e-12) + @test isapprox(m_identity.q2, 0.0; atol=1e-12) + @test isapprox(m_identity.q3, 0.0; atol=1e-12) + + # / (m1 / m2 = m1 * inv(m2)) + m_div = m1 / m2 + m_mult = m1 * inv(m2) + @test isapprox(m_div, m_mult) + + # \ (m1 \ m2 = inv(m1) * m2) + m_backdiv = m1 \ m2 + m_mult_back = inv(m1) * m2 + @test isapprox(m_backdiv, m_mult_back) +end + +@testset "MRP Utils" begin + # 3-4-5 triangle? no, just simple numbers + m = MRP(3.0, 0.0, 4.0) + @test norm(m) == 5.0 + + @test one(MRP) == MRP(0.0, 0.0, 0.0) + @test one(m) == MRP(0.0, 0.0, 0.0) + + @test zero(MRP) == MRP(0.0, 0.0, 0.0) + @test zero(m) == MRP(0.0, 0.0, 0.0) + + # copy + m_copy = copy(m) + @test m_copy == m + + # vect + v = vect(m) + @test v isa SVector{3, Float64} + @test v[1] == m.q1 + @test v[2] == m.q2 + @test v[3] == m.q3 + + # UniformScaling + @test I * m == m + @test m * I == m + @test I / m == inv(m) + @test m / I == m + @test I \ m == m + @test m \ I == inv(m) +end + +@testset "MRP Shadow Rotation" begin + m = MRP(0.1, 0.2, 0.3) + m_shadow = shadow_rotation(m) + + # Check if the shadow rotation is indeed the shadow + @test norm(m_shadow) > 1.0 + @test isapprox(norm(m_shadow), 1/norm(m); atol=1e-12) + + # Check if they represent the same rotation + dcm = mrp_to_dcm(m) + dcm_shadow = mrp_to_dcm(m_shadow) + + @test maximum(abs.(dcm - dcm_shadow)) < 1e-12 +end + +@testset "MRP Random" begin + Random.seed!(123) + m = rand(MRP) + @test m isa MRP{Float64} + + m_float32 = rand(MRP{Float32}) + @test m_float32 isa MRP{Float32} +end + +@testset "MRP Kinematics" begin + # Test analytical derivative against numerical derivative + for T in (Float64,) + # We need a seed to ensure reproducibility + Random.seed!(123) + + m = rand(MRP{T}) + w = @SVector randn(T, 3) + + dm = dmrp(m, w) + + dt = 1e-8 + + # Use DCM mapping for propagation to avoid circular dependency if we were to use dmrp for integration, + # but here we want to verify dmrp. + D = mrp_to_dcm(m) + + # In the existing tests (kinematics.jl), they use: + # dDba = ddcm(Dba, Dba * wba_a) + # Dba = Dba + dDba * Δ + # Dba = orthonormalize(Dba) + + dD = ddcm(D, w) # w is in body frame? Make sure. ddcm says wba_b. dmrp says wba_b. Matches. + D_next = D + dD * dt + D_next = orthonormalize(D_next) + + m_next = dcm_to_mrp(D_next) + + # Check if we switched to the shadow set (norm > 1 or just large jump) + # If m_next is far from m, try shadow + if norm(m_next - m) > 0.1 + m_next = shadow_rotation(m_next) + end + + dm_num = (m_next - m) / dt + + # Check alignment + @test isapprox(dm, dm_num[:]; rtol = 1e-4, atol=1e-6) + end +end + + + + diff --git a/test/runtests.jl b/test/runtests.jl index 3083da8..b07d446 100644 --- a/test/runtests.jl +++ b/test/runtests.jl @@ -103,6 +103,16 @@ println("") end println("") +@time @testset "CRP" verbose = true begin + include("crp.jl") +end +println("") + +@time @testset "MRP" verbose = true begin + include("mrp.jl") +end +println("") + @time @testset "Conversions" verbose = true begin include("./conversions/angle_to_angle.jl") include("./conversions/angle_to_angleaxis.jl")