Skip to content

Commit b53dc3d

Browse files
tahoe-vegastahoe-vegas
authored
✨ CRP and MRP implementation (#36)
1 parent be94e08 commit b53dc3d

24 files changed

Lines changed: 1516 additions & 4 deletions

‎src/ReferenceFrameRotations.jl‎

Lines changed: 15 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -52,6 +52,9 @@ include("dcm.jl")
5252
include("inv_rotations.jl")
5353
include("random.jl")
5454
include("quaternion.jl")
55+
include("crp.jl")
56+
include("mrp.jl")
57+
include("shadow_rotation.jl")
5558

5659
include("deprecations.jl")
5760

@@ -63,13 +66,25 @@ include("./conversions/angle_to_rot.jl")
6366
include("./conversions/angleaxis_to_angle.jl")
6467
include("./conversions/angleaxis_to_dcm.jl")
6568
include("./conversions/angleaxis_to_quat.jl")
69+
include("./conversions/crp_to_angle.jl")
70+
include("./conversions/crp_to_angleaxis.jl")
71+
include("./conversions/crp_to_dcm.jl")
72+
include("./conversions/crp_to_quat.jl")
6673
include("./conversions/api.jl")
6774
include("./conversions/dcm_to_angle.jl")
6875
include("./conversions/dcm_to_angleaxis.jl")
76+
include("./conversions/dcm_to_crp.jl")
77+
include("./conversions/dcm_to_mrp.jl")
6978
include("./conversions/dcm_to_quat.jl")
79+
include("./conversions/mrp_to_angle.jl")
80+
include("./conversions/mrp_to_angleaxis.jl")
81+
include("./conversions/mrp_to_dcm.jl")
82+
include("./conversions/mrp_to_quat.jl")
7083
include("./conversions/quat_to_angle.jl")
7184
include("./conversions/quat_to_angleaxis.jl")
85+
include("./conversions/quat_to_crp.jl")
7286
include("./conversions/quat_to_dcm.jl")
87+
include("./conversions/quat_to_mrp.jl")
7388
include("./conversions/smallangle_to_dcm.jl")
7489
include("./conversions/smallangle_to_quat.jl")
7590
include("./conversions/smallangle_to_rot.jl")

‎src/compose_rotations.jl‎

Lines changed: 6 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -90,6 +90,12 @@ end
9090
@inline compose_rotation(q::Quaternion) = q
9191
@inline compose_rotation(q::Quaternion, qs::Quaternion...) = q * compose_rotation(qs...)
9292

93+
@inline compose_rotation(c::CRP) = c
94+
@inline compose_rotation(c::CRP, cs::CRP...) = compose_rotation(cs...) * c
95+
96+
@inline compose_rotation(m::MRP) = m
97+
@inline compose_rotation(m::MRP, ms::MRP...) = compose_rotation(ms...) * m
98+
9399
# This algorithm was proposed by @Per in
94100
#
95101
# https://discourse.julialang.org/t/improve-the-performance-of-multiplication-of-an-arbitrary-number-of-matrices/10835/24

‎src/conversions/api.jl‎

Lines changed: 32 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -9,6 +9,8 @@
99
Base.convert(::Type{<:DCM}, a::EulerAngles) = angle_to_dcm(a)
1010
Base.convert(::Type{<:DCM}, a::Quaternion) = quat_to_dcm(a)
1111
Base.convert(::Type{<:DCM}, a::EulerAngleAxis) = angleaxis_to_dcm(a)
12+
Base.convert(::Type{<:DCM}, a::CRP) = crp_to_dcm(a)
13+
Base.convert(::Type{<:DCM}, a::MRP) = mrp_to_dcm(a)
1214

1315
# == Conversion to Euler Angles ============================================================
1416

@@ -28,18 +30,48 @@ function Base.convert(::Type{<:_EulerAngleConversion{R}}, a::Quaternion) where R
2830
return quat_to_angle(a, R)
2931
end
3032

33+
function Base.convert(::Type{<:_EulerAngleConversion{R}}, a::CRP) where R
34+
return crp_to_angle(a, R)
35+
end
36+
37+
function Base.convert(::Type{<:_EulerAngleConversion{R}}, a::MRP) where R
38+
return mrp_to_angle(a, R)
39+
end
40+
3141
Base.convert(::Type{<:EulerAngles}, a::DCM) = dcm_to_angle(a, :ZYX)
3242
Base.convert(::Type{<:EulerAngles}, a::EulerAngleAxis) = angleaxis_to_angle(a, :ZYX)
3343
Base.convert(::Type{<:EulerAngles}, a::Quaternion) = quat_to_angle(a, :ZYX)
44+
Base.convert(::Type{<:EulerAngles}, a::CRP) = crp_to_angle(a, :ZYX)
45+
Base.convert(::Type{<:EulerAngles}, a::MRP) = mrp_to_angle(a, :ZYX)
3446

3547
# == Conversions to Euler Angle and Axis ===================================================
3648

3749
Base.convert(::Type{<:EulerAngleAxis}, a::DCM) = dcm_to_angleaxis(a)
3850
Base.convert(::Type{<:EulerAngleAxis}, a::EulerAngles) = angle_to_angleaxis(a)
3951
Base.convert(::Type{<:EulerAngleAxis}, a::Quaternion) = quat_to_angleaxis(a)
52+
Base.convert(::Type{<:EulerAngleAxis}, a::CRP) = crp_to_angleaxis(a)
53+
Base.convert(::Type{<:EulerAngleAxis}, a::MRP) = mrp_to_angleaxis(a)
4054

4155
# == Conversions to Quaternions ============================================================
4256

4357
Base.convert(::Type{<:Quaternion}, a::DCM) = dcm_to_quat(a)
4458
Base.convert(::Type{<:Quaternion}, a::EulerAngles) = angle_to_quat(a)
4559
Base.convert(::Type{<:Quaternion}, a::EulerAngleAxis) = angleaxis_to_quat(a)
60+
Base.convert(::Type{<:Quaternion}, a::CRP) = crp_to_quat(a)
61+
Base.convert(::Type{<:Quaternion}, a::MRP) = mrp_to_quat(a)
62+
63+
# == Conversions to CRP ====================================================================
64+
65+
Base.convert(::Type{<:CRP}, a::DCM) = dcm_to_crp(a)
66+
Base.convert(::Type{<:CRP}, a::Quaternion) = quat_to_crp(a)
67+
Base.convert(::Type{<:CRP}, a::EulerAngles) = dcm_to_crp(angle_to_dcm(a))
68+
Base.convert(::Type{<:CRP}, a::EulerAngleAxis) = dcm_to_crp(angleaxis_to_dcm(a))
69+
Base.convert(::Type{<:CRP}, a::MRP) = dcm_to_crp(mrp_to_dcm(a))
70+
71+
# == Conversions to MRP ====================================================================
72+
73+
Base.convert(::Type{<:MRP}, a::DCM) = dcm_to_mrp(a)
74+
Base.convert(::Type{<:MRP}, a::Quaternion) = quat_to_mrp(a)
75+
Base.convert(::Type{<:MRP}, a::EulerAngles) = dcm_to_mrp(angle_to_dcm(a))
76+
Base.convert(::Type{<:MRP}, a::EulerAngleAxis) = dcm_to_mrp(angleaxis_to_dcm(a))
77+
Base.convert(::Type{<:MRP}, a::CRP) = dcm_to_mrp(crp_to_dcm(a))

‎src/conversions/crp_to_angle.jl‎

Lines changed: 11 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,11 @@
1+
export crp_to_angle
2+
3+
"""
4+
crp_to_angle(c::CRP, rot_seq::Symbol) -> EulerAngles
5+
6+
Convert CRP `c` to Euler Angles with rotation sequence `rot_seq`.
7+
"""
8+
function crp_to_angle(c::CRP, rot_seq::Symbol)
9+
dcm = crp_to_dcm(c)
10+
return dcm_to_angle(dcm, rot_seq)
11+
end

‎src/conversions/crp_to_angleaxis.jl‎

Lines changed: 11 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,11 @@
1+
export crp_to_angleaxis
2+
3+
"""
4+
crp_to_angleaxis(c::CRP) -> EulerAngleAxis
5+
6+
Convert CRP `c` to Euler Angle and Axis.
7+
"""
8+
function crp_to_angleaxis(c::CRP)
9+
q = crp_to_quat(c)
10+
return quat_to_angleaxis(q)
11+
end

‎src/conversions/crp_to_dcm.jl‎

Lines changed: 23 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,23 @@
1+
export crp_to_dcm
2+
3+
"""
4+
crp_to_dcm(c::CRP) -> DCM
5+
6+
Convert CRP `c` to a Direction Cosine Matrix (DCM).
7+
"""
8+
function crp_to_dcm(c::CRP)
9+
q = c
10+
q_sq = q.q1^2 + q.q2^2 + q.q3^2
11+
denom = 1 + q_sq
12+
13+
# Auxiliary variables to reduce computational burden.
14+
q1q2 = q.q1 * q.q2
15+
q1q3 = q.q1 * q.q3
16+
q2q3 = q.q2 * q.q3
17+
18+
return DCM(
19+
(1 + q.q1^2 - q.q2^2 - q.q3^2) / denom, 2 * (q1q2 + q.q3) / denom, 2 * (q1q3 - q.q2) / denom,
20+
2 * (q1q2 - q.q3) / denom, (1 - q.q1^2 + q.q2^2 - q.q3^2) / denom, 2 * (q2q3 + q.q1) / denom,
21+
2 * (q1q3 + q.q2) / denom, 2 * (q2q3 - q.q1) / denom, (1 - q.q1^2 - q.q2^2 + q.q3^2) / denom
22+
)'
23+
end

‎src/conversions/crp_to_quat.jl‎

Lines changed: 13 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,13 @@
1+
export crp_to_quat
2+
3+
"""
4+
crp_to_quat(c::CRP) -> Quaternion
5+
6+
Convert CRP `c` to a Quaternion.
7+
"""
8+
function crp_to_quat(c::CRP)
9+
q_sq = c.q1^2 + c.q2^2 + c.q3^2
10+
β0 = 1 / sqrt(1 + q_sq)
11+
12+
return Quaternion(β0, c.q1 * β0, c.q2 * β0, c.q3 * β0)
13+
end

‎src/conversions/dcm_to_crp.jl‎

Lines changed: 14 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,14 @@
1+
export dcm_to_crp
2+
3+
"""
4+
dcm_to_crp(dcm::DCM) -> CRP
5+
6+
Convert DCM `dcm` to CRP.
7+
"""
8+
function dcm_to_crp(dcm::DCM)
9+
# Convert transformation matrix to quaternion.
10+
q = dcm_to_quat(dcm)
11+
12+
# Convert quaternion to CRP.
13+
return quat_to_crp(q)
14+
end

‎src/conversions/dcm_to_mrp.jl‎

Lines changed: 10 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,10 @@
1+
export dcm_to_mrp
2+
3+
"""
4+
dcm_to_mrp(dcm::DCM) -> MRP
5+
6+
Convert DCM `dcm` to MRP.
7+
"""
8+
function dcm_to_mrp(dcm::DCM)
9+
return quat_to_mrp(dcm_to_quat(dcm))
10+
end

‎src/conversions/mrp_to_angle.jl‎

Lines changed: 11 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,11 @@
1+
export mrp_to_angle
2+
3+
"""
4+
mrp_to_angle(m::MRP, rot_seq::Symbol) -> EulerAngles
5+
6+
Convert MRP `m` to Euler Angles with rotation sequence `rot_seq`.
7+
"""
8+
function mrp_to_angle(m::MRP, rot_seq::Symbol)
9+
dcm = mrp_to_dcm(m)
10+
return dcm_to_angle(dcm, rot_seq)
11+
end

0 commit comments

Comments
 (0)