CRP and MRP implementation

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")

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

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))

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

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

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

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

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

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

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

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

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

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

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

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