Package us.ihmc.euclid.tools

Class RotationMatrixTools

java.lang.Object

us.ihmc.euclid.tools.RotationMatrixTools

public abstract class RotationMatrixTools
extends java.lang.Object

Tools for performing operations on rotation matrices.

Author:

Sylvain Bertrand

Constructor Summary

Constructors

Constructor	Description
RotationMatrixTools()

Method Summary

Modifier and Type	Method	Description
static void	appendPitchRotation(RotationMatrixReadOnly matrixOriginal, double pitch, RotationMatrix matrixToPack)	Append a rotation about the y-axis to matrixOriginal and stores the result in matrixToPack.
static void	appendRollRotation(RotationMatrixReadOnly matrixOriginal, double roll, RotationMatrix matrixToPack)	Append a rotation about the x-axis to matrixOriginal and stores the result in matrixToPack.
static void	appendYawRotation(RotationMatrixReadOnly matrixOriginal, double yaw, RotationMatrix matrixToPack)	Append a rotation about the z-axis to matrixOriginal and stores the result in matrixToPack.
static void	applyPitchRotation(double pitch, Tuple3DReadOnly tupleOriginal, Tuple3DBasics tupleTransformed)	Rotates the given tupleOriginal by a rotation about the y-axis and stores the result in tupleTransformed.
static void	applyRollRotation(double roll, Tuple3DReadOnly tupleOriginal, Tuple3DBasics tupleTransformed)	Rotates the given tupleOriginal by a rotation about the x-axis and stores the result in tupleTransformed.
static void	applyYawRotation(double yaw, Tuple2DReadOnly tupleOriginal, Tuple2DBasics tupleTransformed)	Rotates the given tupleOriginal by a rotation about the z-axis and stores the result in tupleTransformed.
static void	applyYawRotation(double yaw, Tuple3DReadOnly tupleOriginal, Tuple3DBasics tupleTransformed)	Rotates the given tupleOriginal by a rotation about the z-axis and stores the result in tupleTransformed.
static double	distance(RotationMatrixReadOnly m1, RotationMatrixReadOnly m2)	Computes and returns the distance from the rotation matrix m1 to m2.
static void	interpolate(RotationMatrixReadOnly r0, RotationMatrixReadOnly rf, double alpha, RotationMatrix matrixToPack)	Performs a linear interpolation in SO(3) from r0 to rf given the percentage alpha.
static void	multiply(RotationMatrixReadOnly orientation1, boolean inverse1, Orientation3DReadOnly orientation2, boolean inverse2, RotationMatrix matrixToPack)	Performs the multiplication of orientation1 and orientation2 and stores the result in matrixToPack.
static void	multiply(RotationMatrixReadOnly m1, RotationMatrixReadOnly m2, RotationMatrix matrixToPack)	Performs the multiplication: m1 * m2 and stores the result in matrixToPack.
static void	multiply(Orientation3DReadOnly orientation1, boolean inverse1, RotationMatrixReadOnly orientation2, boolean inverse2, RotationMatrix matrixToPack)	Performs the multiplication of orientation1 and orientation2 and stores the result in matrixToPack.
static void	multiply(Orientation3DReadOnly orientation1, boolean inverse1, Orientation3DReadOnly orientation2, boolean inverse2, RotationMatrix matrixToPack)	Performs the multiplication of orientation1 and orientation2 and stores the result in matrixToPack.
static void	multiplyTransposeBoth(RotationMatrixReadOnly m1, RotationMatrixReadOnly m2, RotationMatrix matrixToPack)	Performs the multiplication: m1T * m2T and stores the result in matrixToPack.
static void	multiplyTransposeLeft(RotationMatrixReadOnly m1, RotationMatrixReadOnly m2, RotationMatrix matrixToPack)	Performs the multiplication: m1T * m2 and stores the result in matrixToPack.
static void	multiplyTransposeRight(RotationMatrixReadOnly m1, RotationMatrixReadOnly m2, RotationMatrix matrixToPack)	Performs the multiplication: m1 * m2T and stores the result in matrixToPack.
static void	prependPitchRotation(double pitch, RotationMatrixReadOnly matrixOriginal, RotationMatrix matrixToPack)	Prepend a rotation about the y-axis to matrixOriginal and stores the result in matrixToPack.
static void	prependRollRotation(double roll, RotationMatrixReadOnly matrixOriginal, RotationMatrix matrixToPack)	Prepend a rotation about the x-axis to matrixOriginal and stores the result in matrixToPack.
static void	prependYawRotation(double yaw, RotationMatrixReadOnly matrixOriginal, RotationMatrix matrixToPack)	Prepend a rotation about the z-axis to matrixOriginal and stores the result in matrixToPack.

Methods inherited from class java.lang.Object

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

Constructor Detail

RotationMatrixTools

public RotationMatrixTools()

Method Detail

multiply

public static void multiply(RotationMatrixReadOnly m1,
                            RotationMatrixReadOnly m2,
                            RotationMatrix matrixToPack)

Performs the multiplication: m1 * m2 and stores the result in matrixToPack.

All the matrices can be the same object.

Parameters:

m1 - the first matrix. Not modified.

m2 - the second matrix. Not modified.

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

multiplyTransposeBoth

public static void multiplyTransposeBoth(RotationMatrixReadOnly m1,
                                         RotationMatrixReadOnly m2,
                                         RotationMatrix matrixToPack)

Performs the multiplication: m1^T * m2^T and stores the result in matrixToPack.

All the matrices can be the same object.

Parameters:

m1 - the first matrix. Not modified.

m2 - the second matrix. Not modified.

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

multiplyTransposeLeft

public static void multiplyTransposeLeft(RotationMatrixReadOnly m1,
                                         RotationMatrixReadOnly m2,
                                         RotationMatrix matrixToPack)

Performs the multiplication: m1^T * m2 and stores the result in matrixToPack.

All the matrices can be the same object.

Parameters:

m1 - the first matrix. Not modified.

m2 - the second matrix. Not modified.

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

multiplyTransposeRight

public static void multiplyTransposeRight(RotationMatrixReadOnly m1,
                                          RotationMatrixReadOnly m2,
                                          RotationMatrix matrixToPack)

Performs the multiplication: m1 * m2^T and stores the result in matrixToPack.

All the matrices can be the same object.

Parameters:

m1 - the first matrix. Not modified.

m2 - the second matrix. Not modified.

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

multiply

public static void multiply(Orientation3DReadOnly orientation1,
                            boolean inverse1,
                            Orientation3DReadOnly orientation2,
                            boolean inverse2,
                            RotationMatrix matrixToPack)

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

More precisely, orientation1 and orientation2 are first converted to rotation matrices, then an matrix 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.

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

multiply

public static void multiply(Orientation3DReadOnly orientation1,
                            boolean inverse1,
                            RotationMatrixReadOnly orientation2,
                            boolean inverse2,
                            RotationMatrix matrixToPack)

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

More precisely, orientation1 is first converted to a rotation matrix, then an matrix 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.

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

multiply

public static void multiply(RotationMatrixReadOnly orientation1,
                            boolean inverse1,
                            Orientation3DReadOnly orientation2,
                            boolean inverse2,
                            RotationMatrix matrixToPack)

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

More precisely, orientation2 is first converted to a rotation matrix, then an matrix 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.

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

prependYawRotation

public static void prependYawRotation(double yaw,
                                      RotationMatrixReadOnly matrixOriginal,
                                      RotationMatrix matrixToPack)

Prepend a rotation about the z-axis to matrixOriginal and stores the result in matrixToPack.

All the matrices can be the same object.

                / cos(yaw) -sin(yaw) 0 \
 matrixToPack = | sin(yaw)  cos(yaw) 0 | * matrixOriginal
                \    0         0     1 /
 

Parameters:

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

matrixOriginal - the matrix on which the yaw rotation is appended. Not modified.

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

appendYawRotation

public static void appendYawRotation(RotationMatrixReadOnly matrixOriginal,
                                     double yaw,
                                     RotationMatrix matrixToPack)

Append a rotation about the z-axis to matrixOriginal and stores the result in matrixToPack.

All the matrices can be the same object.

                                 / cos(yaw) -sin(yaw) 0 \
 matrixToPack = matrixOriginal * | sin(yaw)  cos(yaw) 0 |
                                 \    0         0     1 /
 

Parameters:

matrixOriginal - the matrix on which the yaw rotation is appended. Not modified.

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

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

prependPitchRotation

public static void prependPitchRotation(double pitch,
                                        RotationMatrixReadOnly matrixOriginal,
                                        RotationMatrix matrixToPack)

Prepend a rotation about the y-axis to matrixOriginal and stores the result in matrixToPack.

All the matrices can be the same object.

                /  cos(pitch) 0 sin(pitch) \
 matrixToPack = |      0      1     0      | * matrixOriginal
                \ -sin(pitch) 0 cos(pitch) /
 

Parameters:

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

matrixOriginal - the matrix on which the pitch rotation is appended. Not modified.

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

appendPitchRotation

public static void appendPitchRotation(RotationMatrixReadOnly matrixOriginal,
                                       double pitch,
                                       RotationMatrix matrixToPack)

Append a rotation about the y-axis to matrixOriginal and stores the result in matrixToPack.

All the matrices can be the same object.

                                 /  cos(pitch) 0 sin(pitch) \
 matrixToPack = matrixOriginal * |      0      1     0      |
                                 \ -sin(pitch) 0 cos(pitch) /
 

Parameters:

matrixOriginal - the matrix on which the pitch rotation is appended. Not modified.

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

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

prependRollRotation

public static void prependRollRotation(double roll,
                                       RotationMatrixReadOnly matrixOriginal,
                                       RotationMatrix matrixToPack)

Prepend a rotation about the x-axis to matrixOriginal and stores the result in matrixToPack.

All the matrices can be the same object.

                / 1     0          0     \
 matrixToPack = | 0 cos(roll) -sin(roll) | * matrixOriginal
                \ 0 sin(roll)  cos(roll) /
 

Parameters:

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

matrixOriginal - the matrix on which the roll rotation is appended. Not modified.

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

appendRollRotation

public static void appendRollRotation(RotationMatrixReadOnly matrixOriginal,
                                      double roll,
                                      RotationMatrix matrixToPack)

Append a rotation about the x-axis to matrixOriginal and stores the result in matrixToPack.

All the matrices can be the same object.

                                 / 1     0          0     \
 matrixToPack = matrixOriginal * | 0 cos(roll) -sin(roll) |
                                 \ 0 sin(roll)  cos(roll) /
 

Parameters:

matrixOriginal - the matrix on which the roll rotation is appended. Not modified.

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

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

applyYawRotation

public static void applyYawRotation(double yaw,
                                    Tuple3DReadOnly tupleOriginal,
                                    Tuple3DBasics tupleTransformed)

Rotates the given tupleOriginal by a rotation about the z-axis and stores the result in tupleTransformed.

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

                    / cos(yaw) -sin(yaw) 0 \
 tupleTransformed = | sin(yaw)  cos(yaw) 0 | * tupleOriginal
                    \    0         0     1 /
 

Parameters:

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

tupleOriginal - the tuple to be transformed. Not modified.

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

applyYawRotation

public static void applyYawRotation(double yaw,
                                    Tuple2DReadOnly tupleOriginal,
                                    Tuple2DBasics tupleTransformed)

Rotates the given tupleOriginal by a rotation about the z-axis and stores the result in tupleTransformed.

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

 tupleTransformed = / cos(yaw) -sin(yaw) \ * tupleOriginal
                    \ sin(yaw)  cos(yaw) /
 

Parameters:

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

tupleOriginal - the tuple to be transformed. Not modified.

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

applyPitchRotation

public static void applyPitchRotation(double pitch,
                                      Tuple3DReadOnly tupleOriginal,
                                      Tuple3DBasics tupleTransformed)

Rotates the given tupleOriginal by a rotation about the y-axis and stores the result in tupleTransformed.

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

                    /  cos(pitch) 0 sin(pitch) \
 tupleTransformed = |      0      1     0      | * tupleOriginal
                    \ -sin(pitch) 0 cos(pitch) /
 

Parameters:

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

tupleOriginal - the tuple to be transformed. Not modified.

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

applyRollRotation

public static void applyRollRotation(double roll,
                                     Tuple3DReadOnly tupleOriginal,
                                     Tuple3DBasics tupleTransformed)

Rotates the given tupleOriginal by a rotation about the x-axis and stores the result in tupleTransformed.

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

                    / 1     0          0     \
 tupleTransformed = | 0 cos(roll) -sin(roll) | * tupleOriginal
                    \ 0 sin(roll)  cos(roll) /
 

Parameters:

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

tupleOriginal - the tuple to be transformed. Not modified.

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

interpolate

public static void interpolate(RotationMatrixReadOnly r0,
                               RotationMatrixReadOnly rf,
                               double alpha,
                               RotationMatrix matrixToPack)

Performs a linear interpolation in SO(3) from r0 to rf given the percentage alpha.

This is equivalent to but much more computationally expensive than the Spherical Linear Interpolation performed with quaternions, see QuaternionBasics.interpolate(QuaternionReadOnly, QuaternionReadOnly, double).

Parameters:

r0 - the first rotation matrix used in the interpolation. Not modified.

rf - the second rotation matrix used in the interpolation. Not modified.

alpha - the percentage to use for the interpolation. A value of 0 will result in setting matrixToPack to r0, while a value of 1 is equivalent to setting matrixToPack to rf.

matrixToPack - the rotation matrix in which the result of the interpolation is stored. Modified.

distance

public static double distance(RotationMatrixReadOnly m1,
                              RotationMatrixReadOnly m2)

Computes and returns the distance from the rotation matrix m1 to m2.

Parameters:

m1 - the first rotation matrix. Not modified.

m2 - the second rotation matrix. Not modified.

Returns:

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