Library

The Direction Cosine Matrix of type T is a SMatrix{3,3,T,9}, which is a 3x3 static matrix of type T.

struct EulerAngleAxis{T}

The definition of Euler Angle and Axis to represent a 3D rotation.

Fields

• a: The Euler angle [rad].
• v: The unitary vector aligned with the Euler axis.

Constructor

function EulerAngleAxis(a::T1, v::AbstractVector{T2}) where {T1,T2}

Create an Euler Angle and Axis representation structure with angle a [rad] and vector v. Notice that the vector v will not be normalized. The type of the returned structure will be selected according to the input types.

struct EulerAngles{T}

The definition of Euler Angles, which is composed of three angles a1, a2, and a3 together with a rotation sequence rot_seq. The latter is provided by a symbol with three characters, each one indicating the rotation axis of the corresponding angle (for example, :ZYX). The valid values for rot_seq are:

• :XYX, :XYZ, :XZX, :XZY, :YXY, :YXZ, :YZX, :YZY, :ZXY, :ZXZ, :ZYX, and ZYZ.

struct Quaternion{T}

The definition of the quaternion. It has four values of type T. The quaternion representation is:

q0 + q1.i + q2.j + q3.k

function Quaternion(v::AbstractVector)

If the vector v has 3 components, then create a quaternion in which the real part is 0 and the vectorial or imaginary part has the same components of the vector v. In other words:

q = 0 + v[1].i + v[2].j + v[3].k

Otherwise, if the vector v has 4 components, then create a quaternion in which the elements match those of the input vector:

q = v[1] + v[2].i + v[3].j + v[4].k

function Quaternion(r::Number, v::AbstractVector)

Create a quaternion with real part r and vectorial or imaginary part v:

r + v[1].i + v[2].j + v[3].k

function Quaternion(q0::T0, q1::T1, q2::T2, q3::T3) where {T0,T1,T2,T3}

Create the following quaternion:

q0 + q1.i + q2.j + q3.k

in which:

• q0 is the real part of the quaternion.
• q1 is the X component of the quaternion vectorial part.
• q2 is the Y component of the quaternion vectorial part.
• q3 is the Z component of the quaternion vectorial part.

function Quaternion(::UniformScaling,::Quaternion{T}) where T

Create an identity quaternion of type T:

T(1) + T(0).i + T(0).j + T(0).k

function Quaternion(u::UniformScaling{T}) where T
function Quaternion{T}(u::UniformScaling) where T

Create the quaternion u.λ + 0.i + 0.j + 0.k.

@inline function norm(q::Quaternion)

Compute the Euclidean norm of the quaternion q:

sqrt(q0² + q1² + q2² + q3²)

function angle2dcm(angle_r1::Number, angle_r2::Number, angle_r3::Number, rot_seq::Symbol = :ZYX)

Convert the Euler angles angle_r1, angle_r2, and angle_r3 [rad] with the rotation sequence rot_seq to a direction cosine matrix.

The rotation sequence is defined by a :Symbol. The possible values are: :XYX, XYZ, :XZX, :XZY, :YXY, :YXZ, :YZX, :YZY, :ZXY, :ZXZ, :ZYX, and :ZYZ. If no value is specified, then it defaults to :ZYX.

Remarks

This function assigns dcm = A3 * A2 * A1 in which Ai is the DCM related with the i-th rotation, i Є [1,2,3].

Example

dcm = angle2dcm(pi/2, pi/3, pi/4, :ZYX)
3×3 StaticArrays.SArray{Tuple{3,3},Float64,2,9}:
  3.06162e-17  0.5       -0.866025
 -0.707107     0.612372   0.353553
  0.707107     0.612372   0.353553

function angle2dcm(eulerang::EulerAngles)

Convert the Euler angles eulerang (see EulerAngles) to a direction cosine matrix.

Returns

The direction cosine matrix.

Remarks

This function assigns dcm = A3 * A2 * A1 in which Ai is the DCM related with the i-th rotation, i Є [1,2,3].

Example

julia> angle2dcm(EulerAngles(pi/2, pi/3, pi/4, :ZYX))
3×3 StaticArrays.SArray{Tuple{3,3},Float64,2,9}:
  3.06162e-17  0.5       -0.866025
 -0.707107     0.612372   0.353553
  0.707107     0.612372   0.353553

function angle2quat(angle_r1::Number, angle_r2::Number, angle_r3::Number, rot_seq::AbstractString="ZYX")

Convert the Euler angles angle_r1, angle_r2, and angle_r3 [rad] with the rotation sequence rot_seq to a quaternion.

The rotation sequence is defined by a :Symbol. The possible values are: :XYX, XYZ, :XZX, :XZY, :YXY, :YXZ, :YZX, :YZY, :ZXY, :ZXZ, :ZYX, and :ZYZ. If no value is specified, then it defaults to :ZYX.

Remarks

This function assigns q = q1 * q2 * q3 in which qi is the quaternion related with the i-th rotation, i Є [1,2,3].

Example

julia> angle2quat(pi/2, pi/3, pi/4, :ZYX)
Quaternion{Float64}:
  + 0.7010573846499779 - 0.09229595564125714.i + 0.5609855267969309.j + 0.43045933457687935.k

function angle2quat(eulerang::EulerAngles)

Convert the Euler angles eulerang (see EulerAngles) to a quaternion.

Remarks

This function assigns q = q1 * q2 * q3 in which qi is the quaternion related with the i-th rotation, i Є [1,2,3].

Example

julia> angle2quat(pi/2, pi/3, pi/4, :ZYX)
Quaternion{Float64}:
  + 0.7010573846499779 - 0.09229595564125714.i + 0.5609855267969309.j + 0.43045933457687935.k

function angle2rot([T,] angle_r1::Number, angle_r2::Number, angle_r3::Number, rot_seq::Symbol = :ZYX)

Convert the Euler angles eulerang (see EulerAngles) to a rotation description of type T, which can be DCM or Quaternion. If the type T is not specified, then it defaults to DCM.

Example

julia> dcm = angle2rot(EulerAngles(pi/2, pi/3, pi/4, :ZYX))
3×3 StaticArrays.SArray{Tuple{3,3},Float64,2,9}:
  3.06162e-17  0.5       -0.866025
 -0.707107     0.612372   0.353553
  0.707107     0.612372   0.353553

julia> q   = angle2rot(Quaternion,EulerAngles(pi/2, pi/3, pi/4, :ZYX))
Quaternion{Float64}:
  + 0.7010573846499779 - 0.09229595564125714.i + 0.5609855267969309.j +
  0.43045933457687935.k

function angle2rot([T,] angle_r1::Number, angle_r2::Number, angle_r3::Number, rot_seq::Symbol = :ZYX)

Convert the Euler angles angle_r1, angle_r2, and angle_r3 [rad] with the rotation sequence rot_seq to a rotation description of type T, which can be DCM or Quaternion. If the type T is not specified, then it defaults to DCM.

The rotation sequence is defined by a :Symbol. The possible values are: :XYX, XYZ, :XZX, :XZY, :YXY, :YXZ, :YZX, :YZY, :ZXY, :ZXZ, :ZYX, and :ZYZ. If no value is specified, then it defaults to :ZYX.

Example

julia> dcm = angle2rot(pi/2, pi/3, pi/4, :ZYX)
3×3 StaticArrays.SArray{Tuple{3,3},Float64,2,9}:
  3.06162e-17  0.5       -0.866025
 -0.707107     0.612372   0.353553
  0.707107     0.612372   0.353553

julia> q   = angle2rot(Quaternion,pi/2, pi/3, pi/4, :ZYX)
Quaternion{Float64}:
  + 0.7010573846499779 - 0.09229595564125714.i + 0.5609855267969309.j + 0.43045933457687935.k

function angleaxis2quat(angleaxis::EulerAngleAxis)

Convert a Euler angle and Euler axis angleaxis (see EulerAngleAxis) to a quaternion.

Remarks

It is expected that the vector angleaxis.v is unitary. However, no verification is performed inside the function. The user must handle this situation.

Example

julia> v = [1;1;1];

julia> v /= norm(v);

julia> angleaxis2quat(EulerAngleAxis(pi/2,v))
Quaternion{Float64}:
  + 0.7071067811865476 + 0.408248290463863.i + 0.408248290463863.j + 0.408248290463863.k

function angleaxis2quat(a::Number, v::AbstractVector)

Convert the Euler angle a [rad] and Euler axis v, which must be a unit vector, to a quaternion.

Remarks

It is expected that the vector v is unitary. However, no verification is performed inside the function. The user must handle this situation.

Example

julia> v = [1;1;1];

julia> v /= norm(v);

julia> angleaxis2quat(pi/2,v)
Quaternion{Float64}:
  + 0.7071067811865476 + 0.408248290463863.i + 0.408248290463863.j + 0.408248290463863.k

@inline function compose_rotation(R1, [, R2, R3, R4, R5, ...])

Compute a composed rotation using the rotations R1, R2, R3, R4, ..., in the following order:

 First rotation
 |
 |
R1 => R2 => R3 => R4 => ...
       |
       |
       Second rotation

The rotations can be described by Direction Cosine Matrices or Quaternions. Notice, however, that all rotations must be of the same type (DCM or quaternion).

The output will have the same type as the inputs (DCM or quaternion).

Example

julia> D1 = angle2dcm(+pi/3,+pi/4,+pi/5,:ZYX);

julia> D2 = angle2dcm(-pi/5,-pi/4,-pi/3,:XYZ);

julia> compose_rotation(D1,D2)
3×3 StaticArrays.SArray{Tuple{3,3},Float64,2,9}:
 1.0          0.0          5.55112e-17
 0.0          1.0          5.55112e-17
 5.55112e-17  5.55112e-17  1.0

julia> q1 = angle2quat(+pi/3,+pi/4,+pi/5,:ZYX);

julia> q2 = angle2quat(-pi/5,-pi/4,-pi/3,:XYZ);

julia> compose_rotation(q1,q2)
Quaternion{Float64}:
  + 1.0 + 0.0.i + 2.0816681711721685e-17.j + 5.551115123125783e-17.k

function create_rotation_matrix(angle::Number, axis::Symbol = :X)

Compute a rotation matrix that rotates a coordinate frame about the axis axis by the angle angle. The axis must be one of the following symbols: :X, :Y, or :Z.

Example

julia> create_rotation_matrix(pi/2, :X)
3×3 StaticArrays.SArray{Tuple{3,3},Float64,2,9}:
 1.0   0.0          0.0
 0.0   6.12323e-17  1.0
 0.0  -1.0          6.12323e-17

function dcm2angle(dcm::DCM, rot_seq::Symbol=:ZYX)

Convert the DCM dcm to Euler Angles (see EulerAngles) given a rotation sequence rot_seq.

The rotation sequence is defined by a :Symbol. The possible values are: :XYX, XYZ, :XZX, :XZY, :YXY, :YXZ, :YZX, :YZY, :ZXY, :ZXZ, :ZYX, and :ZYZ. If no value is specified, then it defaults to :ZYX.

Example

julia> D = DCM([1. 0. 0.; 0. 0. -1; 0. -1 0.]);

julia> dcm2angle(D,:XYZ)
ReferenceFrameRotations.EulerAngles{Float64}(1.5707963267948966, 0.0, -0.0, :XYZ)

function dcm2quat(dcm::DCM)

Convert the DCM dcm to a quaternion. The type of the quaternion will be automatically selected by the constructor Quaternion to avoid InexactError.

Remarks

By convention, the real part of the quaternion will always be positive. Moreover, the function does not check if dcm is a valid direction cosine matrix. This must be handle by the user.

This algorithm was obtained from:

http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/

Example

julia> dcm = angle2dcm(pi/2,0.0,0.0,:XYZ);

julia> q   = dcm2quat(dcm)
Quaternion{Float64}:
  + 0.7071067811865476 + 0.7071067811865475.i + 0.0.j + 0.0.k

function ddcm(Dba::DCM, wba_b::AbstractArray)

Compute the time-derivative of the DCM dcm that rotates a reference frame a into alignment to the reference frame b in which the angular velocity of b with respect to a, and represented in b, is wba_b.

Returns

The time-derivative of the DCM Dba (3x3 matrix of type SMatrix{3,3}).

Example

julia> D = DCM(Matrix{Float64}(I,3,3));

julia> ddcm(D,[1;0;0])
3×3 StaticArrays.SArray{Tuple{3,3},Float64,2,9}:
 0.0   0.0  0.0
 0.0   0.0  1.0
 0.0  -1.0  0.0

function dquat(qba::Quaternion, wba_b::AbstractVector)

Compute the time-derivative of the quaternion qba that rotates a reference frame a into alignment to the reference frame b in which the angular velocity of b with respect to a, and represented in b, is wba_b.

Example

julia> q = Quaternion(1.0I);

julia> dquat(q,[1;0;0])
Quaternion{Float64}:
  + 0.0 + 0.5.i + 0.0.j + 0.0.k

@inline function inv_rotation(R)

Compute the inverse rotation of R, which can be a Direction Cosine Matrix or Quaternion.

The output will have the same type as R (DCM or quaternion).

Remarks

If R is a DCM, than its transpose is computed instead of its inverse to reduce the computational burden. The both are equal if the DCM has unit norm. This must be verified by the used.

If R is a quaternion, than its conjugate is computed instead of its inverse to reduce the computational burden. The both are equal if the quaternion has unit norm. This must be verified by the used.

Example

julia> D = angle2dcm(+pi/3,+pi/4,+pi/5,:ZYX);

julia> inv_rotation(D)
3×3 StaticArrays.SArray{Tuple{3,3},Float64,2,9}:
  0.353553  -0.492816  0.795068
  0.612372   0.764452  0.201527
 -0.707107   0.415627  0.572061

julia> q = angle2quat(+pi/3,+pi/4,+pi/5,:ZYX);

julia> inv_rotation(q)
Quaternion{Float64}:
  + 0.8200711519756747 - 0.06526868310243991.i - 0.45794027732580056.j - 0.336918398289752.k

function quat2angle(q::Quaternion, rot_seq::Symbol = :ZYX)

Convert the quaternion q to Euler Angles (see EulerAngles) given a rotation sequence rot_seq.

The rotation sequence is defined by a :Symbol. The possible values are: :XYX, XYZ, :XZX, :XZY, :YXY, :YXZ, :YZX, :YZY, :ZXY, :ZXZ, :ZYX, and :ZYZ. If no value is specified, then it defaults to :ZYX.

Example

julia> q = Quaternion(cosd(45/2), sind(45/2), 0, 0);

julia> quat2angle(q,:XYZ)
EulerAngles{Float64}(0.7853981633974484, 0.0, -0.0, :XYZ)

function quat2angleaxis(q::Quaternion{T}) where T

Convert the quaternion q to a Euler angle and axis representation (see EulerAngleAxis).

Remarks

This function will not fail if the quaternion norm is not 1. However, the meaning of the results will not be defined, because the input quaternion does not represent a 3D rotation. The user must handle such situations.

Example

julia> q = Quaternion(cosd(45/2), sind(45/2), 0, 0);

julia> quat2angleaxis(q)
EulerAngleAxis{Float64}(0.7853981633974484, [1.0, 0.0, 0.0])

function quat2dcm(q::Quaternion)

Convert the quaternion q to a Direction Cosine Matrix (DCM).

Example

julia> q = Quaternion(cosd(45/2), sind(45/2), 0, 0);

julia> quat2dcm(q)
3×3 StaticArrays.SArray{Tuple{3,3},Float64,2,9}:
 1.0   0.0       0.0
 0.0   0.707107  0.707107
 0.0  -0.707107  0.707107

function smallangle2dcm(θx::Number, θy::Number, θz::Number)

Create a direction cosine matrix from three small rotations of angles θx, θy, and θz [rad] about the axes X, Y, and Z, respectively.

Remarks

No process of ortho-normalization is performed with the computed DCM.

Example

julia> smallangle2dcm(+0.01, -0.01, +0.01)
3×3 StaticArrays.SArray{Tuple{3,3},Float64,2,9}:
  1.0    0.01  0.01
 -0.01   1.0   0.01
 -0.01  -0.01  1.0

function smallangle2quat(θx::Number, θy::Number, θz::Number)

Create a quaternion from three small rotations of angles θx, θy, and θz [rad] about the axes X, Y, and Z, respectively.

Remarks

The quaternion is normalized.

Example

julia> smallangle2quat(+0.01, -0.01, +0.01)
Quaternion{Float64}:
  + 0.9999625021092433 + 0.004999812510546217.i - 0.004999812510546217.j + 0.004999812510546217.k

function smallangle2rot([T,] θx::Number, θy::Number, θz::Number)

Create a rotation description of type T from three small rotations of angles θx, θy, and θz [rad] about the axes X, Y, and Z, respectively.

The type T of the rotation description can be DCM or Quaternion. If the type T is not specified, then if defaults to DCM.

Returns

The rotation description according to the type T.

Example

julia> dcm = smallangle2rot(+0.01, -0.01, +0.01);

julia> q   = smallangle2rot(Quaternion,+0.01, -0.01, +0.01);

julia> dcm = smallangle2rot(+0.01, -0.01, +0.01)
3×3 StaticArrays.SArray{Tuple{3,3},Float64,2,9}:
  1.0    0.01  0.01
 -0.01   1.0   0.01
 -0.01  -0.01  1.0

julia> q   = smallangle2rot(Quaternion,+0.01, -0.01, +0.01)
Quaternion{Float64}:
  + 0.9999625021092433 + 0.004999812510546217.i - 0.004999812510546217.j + 0.004999812510546217.k

@inline function vect(q::Quaternion)

Return the vectorial or imaginary part of the quaternion q represented by a 3 × 1 vector of type SVector{3}.

@inline function *(v::AbstractVector, q::Quaternion)
@inline function *(q::Quaternion, v::AbstractVector)

Compute the multiplication qv*q or q*qv in which qv is a quaternion with real part 0 and vectorial/imaginary part v (Hamilton product).

@inline function *(u::UniformScaling, q::Quaternion)
@inline function *(q::Quaternion, u::UniformScaling)

Compute qu*q or q*qu (Hamilton product), in which qu is the scaled identity quaternion qu = u.λ * I.

@inline function *(λ::Number, q::Quaternion)
@inline function *(q::Quaternion, λ::Number)

Compute λ*q or q*λ, in which λ is a scalar.

@inline function *(q1::Quaternion, q2::Quaternion)

Compute the quaternion multiplication q1*q2 (Hamilton product).

@inline function +(u::UniformScaling, q::Quaternion)
@inline function +(q::Quaternion, u::UniformScaling)

Compute qu + q or q + qu, in which qu is the scaled identity quaternion qu = u.λ * I.

@inline function +(qa::Quaternion, qb::Quaternion)

Compute qa + qb.

@inline function -(u::UniformScaling, q::Quaternion)
@inline function -(q::Quaternion, u::UniformScaling)

Compute qu - q or q - qu, in which qu is the scaled identity quaternion qu = u.λ * I.

@inline function -(qa::Quaternion, qb::Quaternion)

Compute qa - qb.

@inline function /(u::UniformScaling, q::Quaternion)
@inline function /(q::Quaternion, u::UniformScaling)

Compute qu/q or q/qu (Hamilton product), in which qu is the scaled identity quaternion qu = u.λ * I.

@inline function /(λ::Number, q::Quaternion)
@inline function /(q::Quaternion, λ::Number)

Compute the division λ/q or q/λ, in which λ is a scalar.

@inline /(q1::Quaternion, q2::Quaternion) = q1*inv(q2)

Compute q1*inv(q2) (Hamilton product).

@inline function \(u::UniformScaling, q::Quaternion)
@inline function \(q::Quaternion, u::UniformScaling)

Compute inv(qu)*q or inv(q)*qu (Hamilton product), in which qu is the scaled identity quaternion qu = u.λ * I.

@inline \(q::Quaternion, v::AbstractVector)
@inline \(v::AbstractVector, q::Quaternion)

Compute inv(q)*qv or inv(qv)*q in which qv is a quaternion with real part 0 and vectorial/imaginary part v (Hamilton product).

@inline \(q1::Quaternion, q2::Quaternion) = inv(q1)*q2

Compute inv(q1)*q2.

@inline function conj(q::Quaternion)

Compute the complex conjugate of the quaternion q:

q0 - q1.i - q2.j - q3.k

@inline function copy(q::Quaternion{T}) where T

Create a copy of the quaternion q.

@inline function getindex(q::Quaternion, ::Colon)

Transform the quaternion into a 4x1 vector of type T.

@inline function imag(q::Quaternion)

Return the vectorial or imaginary part of the quaternion q represented by a 3 × 1 vector of type SVector{3}.

@inline function inv(q::Quaternion)

Compute the inverse of the quaternion q:

conj(q)
-------
  |q|²

@inline function real(q::Quaternion)

Return the real part of the quaternion q: q0.

function show(io::IO, q::Quaternion{T}) where T

Print the quaternion q to the stream io.

@inline function zeros(q::Quaternion{T}) where T

Create the null quaternion with the same type T of another quaternion q:

T(0) + T(0).i + T(0).j + T(0).k

Example

julia> q1 = Quaternion{Float32}(cosd(45/2),sind(45/2),0,0);

julia> zeros(q1)
Quaternion{Float32}:
  + 0.0 + 0.0.i + 0.0.j + 0.0.k

@inline function zeros(::Type{Quaternion{T}}) where T

Create the null quaternion of type T:

T(0) + T(0).i + T(0).j + T(0).k

If the type T is omitted, then it defaults to Float64.

Example

julia> zeros(Quaternion{Float32})
Quaternion{Float32}:
  + 0.0 + 0.0.i + 0.0.j + 0.0.k

julia> zeros(Quaternion)
Quaternion{Float64}:
  + 0.0 + 0.0.i + 0.0.j + 0.0.k