Package us.ihmc.euclid.tools

Class QuaternionTools

java.lang.Object

us.ihmc.euclid.tools.QuaternionTools

public abstract class QuaternionTools
extends java.lang.Object

This gathers common mathematical operations involving quaternions.

Author:

Sylvain Bertrand

Field Summary

Fields

Modifier and Type	Field	Description
(package private) static double	EPS

Constructor Summary

Constructors

Constructor	Description
QuaternionTools()

Method Summary

Modifier and Type	Method	Description
static void	addTransform(QuaternionReadOnly quaternion, Tuple3DReadOnly tupleOriginal, Tuple3DBasics tupleTransformed)	Transforms the tuple tupleOriginal using quaternion and adds the result to tupleTransformed.
static void	appendPitchRotation(QuaternionReadOnly quaternionOriginal, double pitch, QuaternionBasics quaternionToPack)	Append a rotation about the y-axis to quaternionOriginal and stores the result in quaternionToPack.
static void	appendRollRotation(QuaternionReadOnly quaternionOriginal, double roll, QuaternionBasics quaternionToPack)	Append a rotation about the x-axis to quaternionOriginal and stores the result in quaternionToPack.
static void	appendYawRotation(QuaternionReadOnly quaternionOriginal, double yaw, QuaternionBasics quaternionToPack)	Append a rotation about the z-axis to quaternionOriginal and stores the result in quaternionToPack.
static double	distancePrecise(QuaternionReadOnly q1, QuaternionReadOnly q2)	Computes the distance between the two given quaternions.
static void	inverseTransform(QuaternionReadOnly quaternion, Matrix3DReadOnly matrixOriginal, Matrix3D matrixTransformed)	Performs the inverse of the transform of the matrix matrixOriginal using quaternion and stores the result in matrixTransformed.
static void	inverseTransform(QuaternionReadOnly quaternion, RotationMatrixReadOnly rotationMatrixOriginal, RotationMatrix rotationMatrixTransformed)	Performs the inverse of the transform of the rotation matrix rotationMatrixOriginal using quaternion and stores the result in rotationMatrixTransformed.
static void	inverseTransform(QuaternionReadOnly quaternion, Tuple2DReadOnly tupleOriginal, Tuple2DBasics tupleTransformed, boolean checkIfTransformInXYPlane)	Performs the inverse of the transform of the tuple tupleOriginal using quaternion and stores the result in tupleTransformed.
static void	inverseTransform(QuaternionReadOnly quaternion, Tuple3DReadOnly tupleOriginal, Tuple3DBasics tupleTransformed)	Performs the inverse of the transform of the tuple tupleOriginal using quaternion and stores the result in tupleTransformed.
static void	inverseTransform(QuaternionReadOnly quaternion, QuaternionReadOnly quaternionOriginal, QuaternionBasics quaternionTransformed)	Performs the inverse of the transform of the quaternion quaternionOriginal using quaternion and stores the result in quaternionTransformed.
static void	inverseTransform(QuaternionReadOnly quaternion, Vector4DReadOnly vectorOriginal, Vector4DBasics vectorTransformed)	Performs the inverse of the transform of the vector part of vectorOriginal using quaternion and stores the result in vectorTransformed.
static void	multiply(Orientation3DReadOnly orientation1, boolean inverse1, Orientation3DReadOnly orientation2, boolean inverse2, QuaternionBasics quaternionToPack)	Performs the multiplication of orientation1 and orientation2 and stores the result in quaternionToPack.
static void	multiply(Orientation3DReadOnly orientation1, boolean inverse1, QuaternionReadOnly orientation2, boolean inverse2, QuaternionBasics quaternionToPack)	Performs the multiplication of orientation1 and orientation2 and stores the result in quaternionToPack.
static void	multiply(QuaternionReadOnly orientation1, boolean inverse1, Orientation3DReadOnly orientation2, boolean inverse2, QuaternionBasics quaternionToPack)	Performs the multiplication of orientation1 and orientation2 and stores the result in quaternionToPack.
static void	multiply(QuaternionReadOnly q1, QuaternionReadOnly q2, QuaternionBasics quaternionToPack)	Performs the multiplication of q1 and q2 and stores the result in quaternionToPack.
static void	multiply(Tuple4DReadOnly t1, Tuple4DReadOnly t2, Vector4DBasics vectorToPack)	Performs the multiplication, in the sense of quaternion multiplication, of t1 and t2 and stores the result in vectorToPack.
static void	multiplyConjugateBoth(QuaternionReadOnly q1, QuaternionReadOnly q2, QuaternionBasics quaternionToPack)	Performs the multiplication of the conjugate of q1 and the conjugate of q2 and stores the result in quaternionToPack.
static void	multiplyConjugateLeft(QuaternionReadOnly q1, QuaternionReadOnly q2, QuaternionBasics quaternionToPack)	Performs the multiplication of q1 conjugated and q2 and stores the result in quaternionToPack.
static void	multiplyConjugateLeft(Tuple4DReadOnly t1, Tuple4DReadOnly t2, Vector4DBasics vectorToPack)	Performs the multiplication, in the sense of quaternion multiplication, of t1 conjugated and t2 and stores the result in vectorToPack.
static void	multiplyConjugateRight(QuaternionReadOnly q1, QuaternionReadOnly q2, QuaternionBasics quaternionToPack)	Performs the multiplication of q1 and q2 conjugated and stores the result in quaternionToPack.
static void	multiplyConjugateRight(Tuple4DReadOnly t1, Tuple4DReadOnly t2, Vector4DBasics vectorToPack)	Performs the multiplication, in the sense of quaternion multiplication, of t1 and t2 conjugated and stores the result in vectorToPack.
(package private) static void	multiplyImpl(double q1x, double q1y, double q1z, double q1s, boolean conjugateQ1, double q2x, double q2y, double q2z, double q2s, boolean conjugateQ2, QuaternionBasics quaternionToPack)	Performs the multiplication of q1 and q2 and stores the result in quaternionToPack.
static void	prependPitchRotation(double pitch, QuaternionReadOnly quaternionOriginal, QuaternionBasics quaternionToPack)	Prepend a rotation about the y-axis to quaternionOriginal and stores the result in quaternionToPack.
static void	prependRollRotation(double roll, QuaternionReadOnly quaternionOriginal, QuaternionBasics quaternionToPack)	Append a rotation about the x-axis to quaternionOriginal and stores the result in quaternionToPack.
static void	prependYawRotation(double yaw, QuaternionReadOnly quaternionOriginal, QuaternionBasics quaternionToPack)	Prepend a rotation about the z-axis to quaternionOriginal and stores the result in quaternionToPack.
static void	subTransform(QuaternionReadOnly quaternion, Tuple3DReadOnly tupleOriginal, Tuple3DBasics tupleTransformed)	Transforms the tuple tupleOriginal using quaternion and subtracts the result to tupleTransformed.
static void	transform(QuaternionReadOnly quaternion, Matrix3DReadOnly matrixOriginal, Matrix3D matrixTransformed)	Transforms the matrix matrixOriginal using quaternion and stores the result in matrixTransformed.
static void	transform(QuaternionReadOnly quaternion, RotationMatrixReadOnly rotationMatrixOriginal, RotationMatrix rotationMatrixTransformed)	Transforms the rotation matrix rotationMatrixOriginal using quaternion and stores the result in rotationMatrixTransformed.
static void	transform(QuaternionReadOnly quaternion, Tuple2DReadOnly tupleOriginal, Tuple2DBasics tupleTransformed, boolean checkIfTransformInXYPlane)	Transforms the tuple tupleOriginal using quaternion and stores the result in tupleTransformed.
static void	transform(QuaternionReadOnly quaternion, Tuple3DReadOnly tupleOriginal, Tuple3DBasics tupleTransformed)	Transforms the tuple tupleOriginal using quaternion and stores the result in tupleTransformed.
static void	transform(QuaternionReadOnly quaternion, QuaternionReadOnly quaternionOriginal, QuaternionBasics quaternionTransformed)	Transforms the quaternion quaternionOriginal using quaternion and stores the result in quaternionTransformed.
static void	transform(QuaternionReadOnly quaternion, Vector4DReadOnly vectorOriginal, Vector4DBasics vectorTransformed)	Transforms the vector part of vectorOriginal using quaternion and stores the result in vectorTransformed.
(package private) static void	transformImpl(double qx, double qy, double qz, double qs, boolean conjugateQuaternion, Matrix3DReadOnly matrixOriginal, Matrix3D matrixTransformed)	Transforms the matrix matrixOriginal using quaternion and stores the result in matrixTransformed.

Methods inherited from class java.lang.Object

clone, equals, finalize, getClass, hashCode, notify, notifyAll, toString, wait, wait, wait

Field Detail

EPS

static final double EPS

See Also:

Constant Field Values

Constructor Detail

QuaternionTools

public QuaternionTools()

Method Detail

multiply

public static void multiply(QuaternionReadOnly q1,
                            QuaternionReadOnly q2,
                            QuaternionBasics quaternionToPack)

Performs the multiplication of q1 and q2 and stores the result in quaternionToPack.

quaternionToPack = q1 * q2

All three arguments can be the same object for in place operations.

Parameters:

q1 - the first quaternion in the multiplication. Not modified.

q2 - the second quaternion in the multiplication. Not modified.

quaternionToPack - the quaternion in which the result is stores. Modified.

multiplyConjugateLeft

public static void multiplyConjugateLeft(QuaternionReadOnly q1,
                                         QuaternionReadOnly q2,
                                         QuaternionBasics quaternionToPack)

Performs the multiplication of q1 conjugated and q2 and stores the result in quaternionToPack.

quaternionToPack = q1* * q2

All three arguments can be the same object for in place operations.

Parameters:

q1 - the first quaternion in the multiplication. Not modified.

q2 - the second quaternion in the multiplication. Not modified.

quaternionToPack - the quaternion in which the result is stores. Modified.

multiplyConjugateRight

public static void multiplyConjugateRight(QuaternionReadOnly q1,
                                          QuaternionReadOnly q2,
                                          QuaternionBasics quaternionToPack)

Performs the multiplication of q1 and q2 conjugated and stores the result in quaternionToPack.

quaternionToPack = q1 * q2*

All three arguments can be the same object for in place operations.

Parameters:

q1 - the first quaternion in the multiplication. Not modified.

q2 - the second quaternion in the multiplication. Not modified.

quaternionToPack - the quaternion in which the result is stores. Modified.

multiplyConjugateBoth

public static void multiplyConjugateBoth(QuaternionReadOnly q1,
                                         QuaternionReadOnly q2,
                                         QuaternionBasics quaternionToPack)

Performs the multiplication of the conjugate of q1 and the conjugate of q2 and stores the result in quaternionToPack.

quaternionToPack = q1* * q2*

All three arguments can be the same object for in place operations.

Parameters:

q1 - the first quaternion in the multiplication. Not modified.

q2 - the second quaternion in the multiplication. Not modified.

quaternionToPack - the quaternion in which the result is stores. Modified.

multiply

public static void multiply(Orientation3DReadOnly orientation1,
                            boolean inverse1,
                            Orientation3DReadOnly orientation2,
                            boolean inverse2,
                            QuaternionBasics quaternionToPack)

Performs the multiplication of orientation1 and orientation2 and stores the result in quaternionToPack.

More precisely, orientation1 and orientation2 are first converted to quaternions, then a quaternion multiplication is performed using the two first arguments as entry.

All three arguments can be the same object for in place operations.

Parameters:

orientation1 - the first orientation in the multiplication. Not modified.

inverse1 - whether the first orientation should be inverted in the multiplication.

orientation2 - the second orientation in the multiplication. Not modified.

inverse2 - whether the second orientation should be inverted in the multiplication.

quaternionToPack - the quaternion in which the result is stored. Modified.

multiply

public static void multiply(Orientation3DReadOnly orientation1,
                            boolean inverse1,
                            QuaternionReadOnly orientation2,
                            boolean inverse2,
                            QuaternionBasics quaternionToPack)

Performs the multiplication of orientation1 and orientation2 and stores the result in quaternionToPack.

More precisely, orientation1 is first converted to a quaternion, then a quaternion multiplication is performed using the two first arguments as entry.

All three arguments can be the same object for in place operations.

Parameters:

orientation1 - the first orientation in the multiplication. Not modified.

inverse1 - whether the first orientation should be inverted in the multiplication.

orientation2 - the second orientation in the multiplication. Not modified.

inverse2 - whether the second orientation should be inverted in the multiplication.

quaternionToPack - the quaternion in which the result is stored. Modified.

multiply

public static void multiply(QuaternionReadOnly orientation1,
                            boolean inverse1,
                            Orientation3DReadOnly orientation2,
                            boolean inverse2,
                            QuaternionBasics quaternionToPack)

Performs the multiplication of orientation1 and orientation2 and stores the result in quaternionToPack.

More precisely, orientation2 is first converted to a quaternion, then a quaternion multiplication is performed using the two first arguments as entry.

All three arguments can be the same object for in place operations.

Parameters:

orientation1 - the first orientation in the multiplication. Not modified.

inverse1 - whether the first orientation should be inverted in the multiplication.

orientation2 - the second orientation in the multiplication. Not modified.

inverse2 - whether the second orientation should be inverted in the multiplication.

quaternionToPack - the quaternion in which the result is stored. Modified.

multiplyImpl

static void multiplyImpl(double q1x,
                         double q1y,
                         double q1z,
                         double q1s,
                         boolean conjugateQ1,
                         double q2x,
                         double q2y,
                         double q2z,
                         double q2s,
                         boolean conjugateQ2,
                         QuaternionBasics quaternionToPack)

Performs the multiplication of q1 and q2 and stores the result in quaternionToPack.

This method is for internal use only.

Provides the option to conjugate either quaternion when multiplying them.

All three arguments can be the same object for in place operations.

Parameters:

q1x - the x-component of the first quaternion in the multiplication. Not modified.

q1y - the y-component of the first quaternion in the multiplication. Not modified.

q1z - the z-component of the first quaternion in the multiplication. Not modified.

q1s - the s-component of the first quaternion in the multiplication. Not modified.

conjugateQ1 - whether to conjugate q1 or not.

q2x - the x-component of the second quaternion in the multiplication. Not modified.

q2y - the y-component of the second quaternion in the multiplication. Not modified.

q2z - the z-component of the second quaternion in the multiplication. Not modified.

q2s - the s-component of the second quaternion in the multiplication. Not modified.

conjugateQ2 - whether to conjugate q2 or not.

quaternionToPack - the quaternion in which the result is stores. Modified.

multiply

public static void multiply(Tuple4DReadOnly t1,
                            Tuple4DReadOnly t2,
                            Vector4DBasics vectorToPack)

Performs the multiplication, in the sense of quaternion multiplication, of t1 and t2 and stores the result in vectorToPack.

vectorToPack = t1 * t2

This

All three arguments can be the same object for in place operations.

Parameters:

t1 - the first tuple in the multiplication. Not modified.

t2 - the second tuple in the multiplication. Not modified.

vectorToPack - the vector in which the result is stores. Modified.

multiplyConjugateLeft

public static void multiplyConjugateLeft(Tuple4DReadOnly t1,
                                         Tuple4DReadOnly t2,
                                         Vector4DBasics vectorToPack)

Performs the multiplication, in the sense of quaternion multiplication, of t1 conjugated and t2 and stores the result in vectorToPack.

vectorToPack = t1* * t2

This

All three arguments can be the same object for in place operations.

Parameters:

t1 - the first tuple in the multiplication. Not modified.

t2 - the second tuple in the multiplication. Not modified.

vectorToPack - the vector in which the result is stores. Modified.

multiplyConjugateRight

public static void multiplyConjugateRight(Tuple4DReadOnly t1,
                                          Tuple4DReadOnly t2,
                                          Vector4DBasics vectorToPack)

Performs the multiplication, in the sense of quaternion multiplication, of t1 and t2 conjugated and stores the result in vectorToPack.

vectorToPack = t1 * t2*

This

All three arguments can be the same object for in place operations.

Parameters:

t1 - the first tuple in the multiplication. Not modified.

t2 - the second tuple in the multiplication. Not modified.

vectorToPack - the vector in which the result is stores. Modified.

transform

public static void transform(QuaternionReadOnly quaternion,
                             Tuple3DReadOnly tupleOriginal,
                             Tuple3DBasics tupleTransformed)

Transforms the tuple tupleOriginal using quaternion and stores the result in tupleTransformed.

Both tuples can be the same object for performing in place transformation.

tupleTransformed = quaternion * tupleOriginal * quaternion^-1

Parameters:

quaternion - the quaternion used to transform the tuple. Not modified.

tupleOriginal - the tuple to transform. Not modified.

tupleTransformed - the tuple in which the result is stored. Modified.

inverseTransform

public static void inverseTransform(QuaternionReadOnly quaternion,
                                    Tuple3DReadOnly tupleOriginal,
                                    Tuple3DBasics tupleTransformed)

Performs the inverse of the transform of the tuple tupleOriginal using quaternion and stores the result in tupleTransformed.

This is equivalent to calling transform(QuaternionReadOnly, Tuple3DReadOnly, Tuple3DBasics) with the inverse of the given quaternion.

Both tuples can be the same object for performing in place transformation.

tupleTransformed = quaternion^-1 * tupleOriginal * quaternion

Parameters:

quaternion - the quaternion used to transform the tuple. Not modified.

tupleOriginal - the tuple to transform. Not modified.

tupleTransformed - the tuple in which the result is stored. Modified.

addTransform

public static void addTransform(QuaternionReadOnly quaternion,
                                Tuple3DReadOnly tupleOriginal,
                                Tuple3DBasics tupleTransformed)

Transforms the tuple tupleOriginal using quaternion and adds the result to tupleTransformed.

Both tuples can be the same object for performing in place transformation.

tupleTransformed = tupleTransformed + quaternion * tupleOriginal * quaternion^-1

Parameters:

quaternion - the quaternion used to transform the tuple. Not modified.

tupleOriginal - the tuple to transform. Not modified.

tupleTransformed - the tuple in which the result is stored. Modified.

subTransform

public static void subTransform(QuaternionReadOnly quaternion,
                                Tuple3DReadOnly tupleOriginal,
                                Tuple3DBasics tupleTransformed)

Transforms the tuple tupleOriginal using quaternion and subtracts the result to tupleTransformed.

Both tuples can be the same object for performing in place transformation.

tupleTransformed = tupleTransformed - quaternion * tupleOriginal * quaternion^-1

Parameters:

quaternion - the quaternion used to transform the tuple. Not modified.

tupleOriginal - the tuple to transform. Not modified.

tupleTransformed - the tuple in which the result is stored. Modified.

transform

public static void transform(QuaternionReadOnly quaternion,
                             Tuple2DReadOnly tupleOriginal,
                             Tuple2DBasics tupleTransformed,
                             boolean checkIfTransformInXYPlane)

Transforms the tuple tupleOriginal using quaternion and stores the result in tupleTransformed.

Both tuples can be the same object for performing in place transformation.

tupleTransformed = quaternion * tupleOriginal * quaternion^-1

Parameters:

quaternion - the quaternion used to transform the tuple. Not modified.

tupleOriginal - the tuple to transform. Not modified.

tupleTransformed - the tuple in which the result is stored. Modified.

checkIfTransformInXYPlane - whether this method should assert that the quaternion represents a transformation in the XY plane.

Throws:

NotAMatrix2DException - if checkIfTransformInXYPlane == true and the quaternion does not represent a transformation in the XY plane.

inverseTransform

public static void inverseTransform(QuaternionReadOnly quaternion,
                                    Tuple2DReadOnly tupleOriginal,
                                    Tuple2DBasics tupleTransformed,
                                    boolean checkIfTransformInXYPlane)

Performs the inverse of the transform of the tuple tupleOriginal using quaternion and stores the result in tupleTransformed.

This is equivalent to calling transform(QuaternionReadOnly, Tuple2DReadOnly, Tuple2DBasics, boolean) with the inverse of the given quaternion.

Both tuples can be the same object for performing in place transformation.

tupleTransformed = quaternion^-1 * tupleOriginal * quaternion

Parameters:

quaternion - the quaternion used to transform the tuple. Not modified.

tupleOriginal - the tuple to transform. Not modified.

tupleTransformed - the tuple in which the result is stored. Modified.

checkIfTransformInXYPlane - whether this method should assert that the quaternion represents a transformation in the XY plane.

Throws:

NotAMatrix2DException - if checkIfTransformInXYPlane == true and the quaternion does not represent a transformation in the XY plane.

transform

public static void transform(QuaternionReadOnly quaternion,
                             QuaternionReadOnly quaternionOriginal,
                             QuaternionBasics quaternionTransformed)

Transforms the quaternion quaternionOriginal using quaternion and stores the result in quaternionTransformed.

Both quaternionOriginal and quaternionTransformed can be the same object for performing in place transformation.

quaternionTransformed = quaternion * quaternionOriginal

Note that this transformation is equivalent to concatenating the orientations of quaternion and quaternionOriginal.

Parameters:

quaternion - the quaternion used to transform the quaternion. Not modified.

quaternionOriginal - the quaternion to transform. Not modified.

quaternionTransformed - the quaternion in which the result is stored. Modified.

inverseTransform

public static void inverseTransform(QuaternionReadOnly quaternion,
                                    QuaternionReadOnly quaternionOriginal,
                                    QuaternionBasics quaternionTransformed)

Performs the inverse of the transform of the quaternion quaternionOriginal using quaternion and stores the result in quaternionTransformed.

This is equivalent to calling transform(QuaternionReadOnly, QuaternionReadOnly, QuaternionBasics) with the inverse of the given quaternion.

Both quaternionOriginal and quaternionTransformed can be the same object for performing in place transformation.

quaternionTransformed = quaternion^-1 * quaternionOriginal

Parameters:

quaternion - the quaternion used to transform the quaternion. Not modified.

quaternionOriginal - the quaternion to transform. Not modified.

quaternionTransformed - the quaternion in which the result is stored. Modified.

transform

public static void transform(QuaternionReadOnly quaternion,
                             Vector4DReadOnly vectorOriginal,
                             Vector4DBasics vectorTransformed)

Transforms the vector part of vectorOriginal using quaternion and stores the result in vectorTransformed.

Both vectors can be the same object for performing in place transformation.

vectorTransformed.s = vecorOriginal.s
vectorTransformed.xyz = quaternion * vectorOriginal.xyz * quaternion^-1

Parameters:

quaternion - the quaternion used to transform the vector. Not modified.

vectorOriginal - the vector to transform. Not modified.

vectorTransformed - the vector in which the result is stored. Modified.

inverseTransform

public static void inverseTransform(QuaternionReadOnly quaternion,
                                    Vector4DReadOnly vectorOriginal,
                                    Vector4DBasics vectorTransformed)

Performs the inverse of the transform of the vector part of vectorOriginal using quaternion and stores the result in vectorTransformed.

This is equivalent to calling transform(QuaternionReadOnly, Tuple3DReadOnly, Tuple3DBasics) with the inverse of the given quaternion.

Both vectors can be the same object for performing in place transformation.

vectorTransformed.s = vecorOriginal.s
vectorTransformed.xyz = quaternion^-1 * vectorOriginal.xyz * quaternion

Parameters:

quaternion - the quaternion used to transform the vector. Not modified.

vectorOriginal - the vector to transform. Not modified.

vectorTransformed - the vector in which the result is stored. Modified.

transform

public static void transform(QuaternionReadOnly quaternion,
                             Matrix3DReadOnly matrixOriginal,
                             Matrix3D matrixTransformed)

Transforms the matrix matrixOriginal using quaternion and stores the result in matrixTransformed.

Both matrices can be the same object for performing in place transformation.

matrixTransformed = R(quaternion) * matrixOriginal * R(quaternion)^-1
where R(quaternion) is the function to convert a quaternion into a 3-by-3 rotation matrix.

Parameters:

quaternion - the quaternion used to transform the matrix. Not modified.

matrixOriginal - the matrix to transform. Not modified.

matrixTransformed - the matrix in which the result is stored. Modified.

inverseTransform

public static void inverseTransform(QuaternionReadOnly quaternion,
                                    Matrix3DReadOnly matrixOriginal,
                                    Matrix3D matrixTransformed)

Performs the inverse of the transform of the matrix matrixOriginal using quaternion and stores the result in matrixTransformed.

This is equivalent to calling transform(QuaternionReadOnly, Matrix3DReadOnly, Matrix3D) with the inverse of the given quaternion.

Both matrices can be the same object for performing in place transformation.

matrixTransformed = R(quaternion)^-1 * matrixOriginal * R(quaternion)
where R(quaternion) is the function to convert a quaternion into a 3-by-3 rotation matrix.

Parameters:

quaternion - the quaternion used to transform the matrix. Not modified.

matrixOriginal - the matrix to transform. Not modified.

matrixTransformed - the matrix in which the result is stored. Modified.

transformImpl

static void transformImpl(double qx,
                          double qy,
                          double qz,
                          double qs,
                          boolean conjugateQuaternion,
                          Matrix3DReadOnly matrixOriginal,
                          Matrix3D matrixTransformed)

Transforms the matrix matrixOriginal using quaternion and stores the result in matrixTransformed.

This method is for internal use only.

Both matrices can be the same object for performing in place transformation.

matrixTransformed = R(quaternion) * matrixOriginal * R(quaternion)^-1
where R(quaternion) is the function to convert a quaternion into a 3-by-3 rotation matrix.

Parameters:

qx - the x-component of the quaternion used to transform the matrix.

qy - the y-component of the quaternion used to transform the matrix.

qz - the z-component of the quaternion used to transform the matrix.

qs - the s-component of the quaternion used to transform the matrix.

conjugateQuaternion - whether to conjugate the quaternion or not.

matrixOriginal - the matrix to transform. Not modified.

matrixTransformed - the matrix in which the result is stored. Modified.

transform

public static void transform(QuaternionReadOnly quaternion,
                             RotationMatrixReadOnly rotationMatrixOriginal,
                             RotationMatrix rotationMatrixTransformed)

Transforms the rotation matrix rotationMatrixOriginal using quaternion and stores the result in rotationMatrixTransformed.

Both rotation matrices can be the same object for performing in place transformation.

rotationMatrixTransformed = R(quaternion) * rotationMatrixOriginal
where R(quaternion) is the function to convert a quaternion into a 3-by-3 rotation matrix.

Note that this transformation is equivalent to concatenating the orientations of quaternion and rotationMatrixOriginal.

Parameters:

quaternion - the quaternion used to transform the rotation matrix. Not modified.

rotationMatrixOriginal - the rotation matrix to transform. Not modified.

rotationMatrixTransformed - the rotation matrix in which the result is stored. Modified.

inverseTransform

public static void inverseTransform(QuaternionReadOnly quaternion,
                                    RotationMatrixReadOnly rotationMatrixOriginal,
                                    RotationMatrix rotationMatrixTransformed)

Performs the inverse of the transform of the rotation matrix rotationMatrixOriginal using quaternion and stores the result in rotationMatrixTransformed.

This is equivalent to calling transform(QuaternionReadOnly, RotationMatrixReadOnly, RotationMatrix) with the inverse of the given quaternion.

Both rotation matrices can be the same object for performing in place transformation.

rotationMatrixTransformed = R(quaternion)^-1 * rotationMatrixOriginal
where R(quaternion) is the function to convert a quaternion into a 3-by-3 rotation matrix.

Note that this transformation is equivalent to concatenating the orientations of quaternion and rotationMatrixOriginal.

Parameters:

quaternion - the quaternion used to transform the rotation matrix. Not modified.

rotationMatrixOriginal - the rotation matrix to transform. Not modified.

rotationMatrixTransformed - the rotation matrix in which the result is stored. Modified.

prependYawRotation

public static void prependYawRotation(double yaw,
                                      QuaternionReadOnly quaternionOriginal,
                                      QuaternionBasics quaternionToPack)

Prepend a rotation about the z-axis to quaternionOriginal and stores the result in quaternionToPack.

All the quaternions can be the same object.

                    / qx =     0      \
 quaternionToPack = | qy =     0      | * quaternionOriginal
                    | qz = sin(yaw/2) |
                    \ qs = cos(yaw/2) /
 

Parameters:

yaw - the angle to rotate about the z-axis.

quaternionOriginal - the quaternion on which the yaw rotation is prepended. Not modified.

quaternionToPack - the quaternion in which the result is stored. Modified.

appendYawRotation

public static void appendYawRotation(QuaternionReadOnly quaternionOriginal,
                                     double yaw,
                                     QuaternionBasics quaternionToPack)

Append a rotation about the z-axis to quaternionOriginal and stores the result in quaternionToPack.

All the quaternions can be the same object.

                                         / qx =     0      \
 quaternionToPack = quaternionOriginal * | qy =     0      |
                                         | qz = sin(yaw/2) |
                                         \ qs = cos(yaw/2) /
 

Parameters:

quaternionOriginal - the quaternion on which the yaw rotation is appended. Not modified.

yaw - the angle to rotate about the z-axis.

quaternionToPack - the quaternion in which the result is stored. Modified.

prependPitchRotation

public static void prependPitchRotation(double pitch,
                                        QuaternionReadOnly quaternionOriginal,
                                        QuaternionBasics quaternionToPack)

Prepend a rotation about the y-axis to quaternionOriginal and stores the result in quaternionToPack.

All the quaternions can be the same object.

                    / qx =      0       \
 quaternionToPack = | qy = sin(pitch/2) | * quaternionOriginal
                    | qz =      0       |
                    \ qs = cos(pitch/2) /
 

Parameters:

pitch - the angle to rotate about the y-axis.

quaternionOriginal - the quaternion on which the pitch rotation is prepended. Not modified.

quaternionToPack - the quaternion in which the result is stored. Modified.

appendPitchRotation

public static void appendPitchRotation(QuaternionReadOnly quaternionOriginal,
                                       double pitch,
                                       QuaternionBasics quaternionToPack)

Append a rotation about the y-axis to quaternionOriginal and stores the result in quaternionToPack.

All the quaternions can be the same object.

                                         / qx =      0       \
 quaternionToPack = quaternionOriginal * | qy = sin(pitch/2) |
                                         | qz =      0       |
                                         \ qs = cos(pitch/2) /
 

Parameters:

quaternionOriginal - the quaternion on which the pitch rotation is appended. Not modified.

pitch - the angle to rotate about the y-axis.

quaternionToPack - the quaternion in which the result is stored. Modified.

prependRollRotation

public static void prependRollRotation(double roll,
                                       QuaternionReadOnly quaternionOriginal,
                                       QuaternionBasics quaternionToPack)

Append a rotation about the x-axis to quaternionOriginal and stores the result in quaternionToPack.

All the quaternions can be the same object.

                    / qx = sin(roll/2) \
 quaternionToPack = | qy =      0      | * quaternionOriginal
                    | qz =      0      |
                    \ qs = cos(roll/2) /
 

Parameters:

roll - the angle to rotate about the x-axis.

quaternionOriginal - the quaternion on which the roll rotation is prepended. Not modified.

quaternionToPack - the quaternion in which the result is stored. Modified.

appendRollRotation

public static void appendRollRotation(QuaternionReadOnly quaternionOriginal,
                                      double roll,
                                      QuaternionBasics quaternionToPack)

Append a rotation about the x-axis to quaternionOriginal and stores the result in quaternionToPack.

All the quaternions can be the same object.

                                         / qx = sin(roll/2) \
 quaternionToPack = quaternionOriginal * | qy =      0      |
                                         | qz =      0      |
                                         \ qs = cos(roll/2) /
 

Parameters:

quaternionOriginal - the quaternion on which the roll rotation is appended. Not modified.

roll - the angle to rotate about the x-axis.

quaternionToPack - the quaternion in which the result is stored. Modified.

distancePrecise

public static double distancePrecise(QuaternionReadOnly q1,
                                     QuaternionReadOnly q2)

Computes the distance between the two given quaternions.

Parameters:

q1 - the quaternion to be used in the comparison. Not modified.

q2 - the quaternion to be used in the comparison. Not modified.

Returns:

the angle representing the distance between the two quaternions. It is contained in [0, 2pi]