us.ihmc.euclid.tools

Class Matrix3DTools

java.lang.Object

us.ihmc.euclid.tools.Matrix3DTools

public abstract class Matrix3DTools
extends java.lang.Object

Tools for performing operations on 3D matrices.

Constructor Summary

Constructors

Constructor and Description
Matrix3DTools()

Method Summary

Modifier and Type	Method and Description
static void	addTransform(Matrix3DReadOnly matrix, Tuple3DReadOnly tupleOriginal, Tuple3DBasics tupleTransformed) Performs a transformation of tupleOriginal using the given matrix and add the result to tupleTransformed:
static java.lang.ArrayIndexOutOfBoundsException	columnOutOfBoundsException(int maxColumnIndex, int column) Create an ArrayIndexOutOfBoundsException for a bad column index.
static void	inverseTransform(Matrix3DReadOnly matrix, Matrix3DReadOnly matrixOriginal, Matrix3D matrixTransformed) Undoes the transformation on matrixOriginal using matrix and stores the result in matrixTransformed:
static void	inverseTransform(Matrix3DReadOnly matrix, Tuple2DReadOnly tupleOriginal, Tuple2DBasics tupleTransformed, boolean checkIfTransformInXYPlane) Undoes the transformation of tupleOriginal using the given matrix and stores the result in tupleTransformed:
static void	inverseTransform(Matrix3DReadOnly matrix, Tuple3DReadOnly tupleOriginal, Tuple3DBasics tupleTransformed) Undoes the transformation of tupleOriginal using the given matrix and stores the result in tupleTransformed:
static void	inverseTransform(Matrix3DReadOnly matrix, Vector4DReadOnly vectorOriginal, Vector4DBasics vectorTransformed) Undoes the transformation on the vector part of vectorOriginal using the given matrix and stores the result in vectorTransformed:
static boolean	invert(Matrix3D matrixToInvert) Performs an in-place inversion of the given matrix such that: m = m-1.
static boolean	invert(Matrix3DReadOnly matrix, Matrix3D inverseToPack) Computes the inverse of matrix and stores the result in inverseToPack.
static void	multiply(Matrix3DReadOnly m1, Matrix3DReadOnly m2, Matrix3D matrixToPack) Performs the multiplication: m1 * m2 and stores the result in matrixToPack.
static void	multiplyInvertBoth(Matrix3DReadOnly m1, Matrix3DReadOnly m2, Matrix3D matrixToPack) Performs the multiplication: m1-1 * m2-1 and stores the result in matrixToPack.
static void	multiplyInvertLeft(Matrix3DReadOnly m1, Matrix3DReadOnly m2, Matrix3D matrixToPack) Performs the multiplication: m1-1 * m2 and stores the result in matrixToPack.
static void	multiplyInvertLeft(RotationMatrixReadOnly m1, Matrix3DReadOnly m2, Matrix3D matrixToPack) Performs the multiplication: m1-1 * m2 and stores the result in matrixToPack.
static void	multiplyInvertLeft(RotationScaleMatrixReadOnly m1, Matrix3DReadOnly m2, Matrix3D matrixToPack) Performs the multiplication: m1-1 * m2 and stores the result in matrixToPack.
static void	multiplyInvertRight(Matrix3DReadOnly m1, Matrix3DReadOnly m2, Matrix3D matrixToPack) Performs the multiplication: m1 * m2-1 and stores the result in matrixToPack.
static void	multiplyInvertRight(Matrix3DReadOnly m1, RotationMatrixReadOnly m2, Matrix3D matrixToPack) Performs the multiplication: m1 * m2-1 and stores the result in matrixToPack.
static void	multiplyInvertRight(Matrix3DReadOnly m1, RotationScaleMatrixReadOnly m2, Matrix3D matrixToPack) Performs the multiplication: m1 * m2-1 and stores the result in matrixToPack.
static void	multiplyTransposeBoth(Matrix3DReadOnly m1, Matrix3DReadOnly m2, Matrix3D matrixToPack) Performs the multiplication: m1T * m2T and stores the result in matrixToPack.
static void	multiplyTransposeLeft(Matrix3DReadOnly m1, Matrix3DReadOnly m2, Matrix3D matrixToPack) Performs the multiplication: m1T * m2 and stores the result in matrixToPack.
static void	multiplyTransposeRight(Matrix3DReadOnly m1, Matrix3DReadOnly m2, Matrix3D matrixToPack) Performs the multiplication: m1 * m2T and stores the result in matrixToPack.
static void	normalize(Matrix3DBasics matrixToNormalize) Orthonormalization of the given matrix using the Gram-Schmidt method.
static java.lang.ArrayIndexOutOfBoundsException	rowOutOfBoundsException(int maxRowIndex, int row) Create an ArrayIndexOutOfBoundsException for a bad row index.
static void	subTransform(Matrix3DReadOnly matrix, Tuple3DReadOnly tupleOriginal, Tuple3DBasics tupleTransformed) Performs a transformation of tupleOriginal using the given matrix and subtract the result to tupleTransformed:
static void	transform(Matrix3DReadOnly matrix, Matrix3DReadOnly matrixOriginal, Matrix3D matrixTransformed) Performs a transformation of matrixOriginal using matrix and stores the result in matrixTransformed:
static void	transform(Matrix3DReadOnly matrix, Tuple2DReadOnly tupleOriginal, Tuple2DBasics tupleTransformed, boolean checkIfTransformInXYPlane) Performs a transformation of tupleOriginal using the given matrix and stores the result in tupleTransformed:
static void	transform(Matrix3DReadOnly matrix, Tuple3DReadOnly tupleOriginal, Tuple3DBasics tupleTransformed) Performs a transformation of tupleOriginal using the given matrix and stores the result in tupleTransformed:
static void	transform(Matrix3DReadOnly matrix, Vector4DReadOnly vectorOriginal, Vector4DBasics vectorTransformed) Performs a transformation on the vector part of vectorOriginal using the given matrix and stores the result in vectorTransformed:

Methods inherited from class java.lang.Object

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

Constructor Detail

Matrix3DTools

public Matrix3DTools()

Method Detail

invert

public static boolean invert(Matrix3D matrixToInvert)

Performs an in-place inversion of the given matrix such that: m = m^-1.

Parameters:

matrixToInvert - the matrix to invert. Modified.

Returns:

true if the inversion succeeds, false if the matrix is singular.

invert

public static boolean invert(Matrix3DReadOnly matrix,
                             Matrix3D inverseToPack)

Computes the inverse of matrix and stores the result in inverseToPack.

The matrices can be the same object.

Parameters:

matrix - the matrix to compute the inverse of. Not modified.

inverseToPack - the result to pack. Modified.

Returns:

true if the inversion succeeds, false if the matrix is singular.

multiply

public static void multiply(Matrix3DReadOnly m1,
                            Matrix3DReadOnly m2,
                            Matrix3D 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(Matrix3DReadOnly m1,
                                         Matrix3DReadOnly m2,
                                         Matrix3D 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.

multiplyInvertBoth

public static void multiplyInvertBoth(Matrix3DReadOnly m1,
                                      Matrix3DReadOnly m2,
                                      Matrix3D matrixToPack)

Performs the multiplication: m1^-1 * m2^-1 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.

Throws:

SingularMatrixException - if the matrix m2 * m1 is not invertible.

multiplyTransposeLeft

public static void multiplyTransposeLeft(Matrix3DReadOnly m1,
                                         Matrix3DReadOnly m2,
                                         Matrix3D 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.

multiplyInvertLeft

public static void multiplyInvertLeft(Matrix3DReadOnly m1,
                                      Matrix3DReadOnly m2,
                                      Matrix3D matrixToPack)

Performs the multiplication: m1^-1 * 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.

Throws:

SingularMatrixException - if m1 is not invertible.

multiplyInvertLeft

public static void multiplyInvertLeft(RotationMatrixReadOnly m1,
                                      Matrix3DReadOnly m2,
                                      Matrix3D matrixToPack)

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

m2 and matrixToPack can be the same object. m1 and m2 can be the same object.

This operation uses the property:
R^-1 = R^T
of a rotation matrix preventing to actually compute the inverse of the rotation matrix.

Parameters:

m1 - the first matrix. Not modified.

m2 - the second matrix. Not modified.

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

multiplyInvertLeft

public static void multiplyInvertLeft(RotationScaleMatrixReadOnly m1,
                                      Matrix3DReadOnly m2,
                                      Matrix3D matrixToPack)

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

m2 and matrixToPack can be the same object. m1 and m2 can be the same object.

This operation uses the property:
(R * S)^-1 = S^-1 * R^T
of the rotation-scale matrix preventing to actually compute its inverse.

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(Matrix3DReadOnly m1,
                                          Matrix3DReadOnly m2,
                                          Matrix3D 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.

multiplyInvertRight

public static void multiplyInvertRight(Matrix3DReadOnly m1,
                                       Matrix3DReadOnly m2,
                                       Matrix3D matrixToPack)

Performs the multiplication: m1 * m2^-1 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.

Throws:

SingularMatrixException - if m2 is not invertible.

multiplyInvertRight

public static void multiplyInvertRight(Matrix3DReadOnly m1,
                                       RotationMatrixReadOnly m2,
                                       Matrix3D matrixToPack)

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

m1 and matrixToPack can be the same object. m1 and m2 can be the same object.

This operation uses the property:
R^-1 = R^T
of a rotation matrix preventing to actually compute the inverse of the rotation matrix.

Parameters:

m1 - the first matrix. Not modified.

m2 - the second matrix. Not modified.

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

multiplyInvertRight

public static void multiplyInvertRight(Matrix3DReadOnly m1,
                                       RotationScaleMatrixReadOnly m2,
                                       Matrix3D matrixToPack)

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

m1 and matrixToPack can be the same object. m1 and m2 can be the same object.

This operation uses the property:
(R * S)^-1 = S^-1 * R^T
of the rotation-scale matrix preventing to actually compute its inverse.

Parameters:

m1 - the first matrix. Not modified.

m2 - the second matrix. Not modified.

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

normalize

public static void normalize(Matrix3DBasics matrixToNormalize)

Orthonormalization of the given matrix using the Gram-Schmidt method.

Parameters:

matrixToNormalize - the matrix to normalize. Modified.

transform

public static void transform(Matrix3DReadOnly matrix,
                             Tuple3DReadOnly tupleOriginal,
                             Tuple3DBasics tupleTransformed)

Performs a transformation of tupleOriginal using the given matrix and stores the result in tupleTransformed:

tupleTransformed = matrix * tupleOriginal.

Both tuples can be the same instance to perform in-place transformation.

Parameters:

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

tupleOriginal - the original tuple to use for the transformation. Not modified.

tupleTransformed - the tuple used to store the result of the transformation. Modified.

addTransform

public static void addTransform(Matrix3DReadOnly matrix,
                                Tuple3DReadOnly tupleOriginal,
                                Tuple3DBasics tupleTransformed)

Performs a transformation of tupleOriginal using the given matrix and add the result to tupleTransformed:

tupleTransformed = tupleTransformed + matrix * tupleOriginal.

Both tuples can be the same instance to perform in-place transformation.

Parameters:

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

tupleOriginal - the original tuple to use for the transformation. Not modified.

tupleTransformed - the tuple to which the result of the transformation is added to. Modified.

subTransform

public static void subTransform(Matrix3DReadOnly matrix,
                                Tuple3DReadOnly tupleOriginal,
                                Tuple3DBasics tupleTransformed)

Performs a transformation of tupleOriginal using the given matrix and subtract the result to tupleTransformed:

tupleTransformed = tupleTransformed - matrix * tupleOriginal.

Both tuples can be the same instance to perform in-place transformation.

Parameters:

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

tupleOriginal - the original tuple to use for the transformation. Not modified.

tupleTransformed - the tuple to which the result of the transformation is added to. Modified.

transform

public static void transform(Matrix3DReadOnly matrix,
                             Tuple2DReadOnly tupleOriginal,
                             Tuple2DBasics tupleTransformed,
                             boolean checkIfTransformInXYPlane)

Performs a transformation of tupleOriginal using the given matrix and stores the result in tupleTransformed:

tupleTransformed = matrix * tupleOriginal.

Before the transformation is performed, if checkIfTransformInXYPlane equals true, this verify that the matrix is a 2D transformation matrix using Matrix3DReadOnly.checkIfMatrix2D().

Both tuples can be the same instance to perform in-place transformation.

Parameters:

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

tupleOriginal - the original tuple to use for the transformation. Not modified.

tupleTransformed - the tuple used to stored the result of the transformation. Modified.

checkIfTransformInXYPlane - whether Matrix3DReadOnly.checkIfMatrix2D() needs to be called on the matrix.

Throws:

NotAMatrix2DException - if the matrix is not a 2D matrix and checkIfTransformInXYPlane is true.

transform

public static void transform(Matrix3DReadOnly matrix,
                             Vector4DReadOnly vectorOriginal,
                             Vector4DBasics vectorTransformed)

Performs a transformation on the vector part of vectorOriginal using the given matrix and stores the result in vectorTransformed:

vectorTransformed.s = vectorOriginal.s. vectorTransformed.xyz = matrix * vectorOriginal.xyz.

Both vectors can be the same instance to perform in-place transformation.

Parameters:

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

vectorOriginal - the original vector to use for the transformation. Not modified.

vectorTransformed - the vector used to stored the result of the transformation. Modified.

Throws:

NotAMatrix2DException - if the matrix is not a 2D matrix and checkIfTransformInXYPlane is true.

transform

public static void transform(Matrix3DReadOnly matrix,
                             Matrix3DReadOnly matrixOriginal,
                             Matrix3D matrixTransformed)

Performs a transformation of matrixOriginal using matrix and stores the result in matrixTransformed:

matrixTransformed = matrix * matrixOriginal * matrix^-1.

WARNING: This is different from concatenating orientations.

matrixOriginal and matrixTransformed can be the same instance to perform in-place transformation.

Parameters:

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

matrixOriginal - the original matrix to use for the transformation. Not modified.

matrixTransformed - the matrix used to stored the result of the transformation. Modified.

inverseTransform

public static void inverseTransform(Matrix3DReadOnly matrix,
                                    Tuple3DReadOnly tupleOriginal,
                                    Tuple3DBasics tupleTransformed)

Undoes the transformation of tupleOriginal using the given matrix and stores the result in tupleTransformed:

tupleTransformed = matrix^-1 * tupleOriginal.

Both tuples can be the same instance to perform in-place transformation.

Parameters:

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

tupleOriginal - the original tuple to use for the transformation. Not modified.

tupleTransformed - the tuple used to store the result of the transformation. Modified.

Throws:

SingularMatrixException - if matrix is not invertible.

inverseTransform

public static void inverseTransform(Matrix3DReadOnly matrix,
                                    Tuple2DReadOnly tupleOriginal,
                                    Tuple2DBasics tupleTransformed,
                                    boolean checkIfTransformInXYPlane)

Undoes the transformation of tupleOriginal using the given matrix and stores the result in tupleTransformed:

tupleTransformed = matrix^-1 * tupleOriginal.

Before the transformation is performed, if checkIfTransformInXYPlane equals true, this verify that the matrix is a 2D transformation matrix using Matrix3DReadOnly.checkIfMatrix2D().

Both tuples can be the same instance to perform in-place transformation.

Parameters:

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

tupleOriginal - the original tuple to use for the transformation. Not modified.

tupleTransformed - the tuple used to stored the result of the transformation. Modified.

checkIfTransformInXYPlane - whether Matrix3DReadOnly.checkIfMatrix2D() needs to be called on the matrix.

Throws:

NotAMatrix2DException - if the matrix is not a 2D matrix and checkIfTransformInXYPlane is true.

SingularMatrixException - if matrix is not invertible.

inverseTransform

public static void inverseTransform(Matrix3DReadOnly matrix,
                                    Vector4DReadOnly vectorOriginal,
                                    Vector4DBasics vectorTransformed)

Undoes the transformation on the vector part of vectorOriginal using the given matrix and stores the result in vectorTransformed:

vectorTransformed.s = vectorOriginal.s. vectorTransformed.xyz = matrix^-1 * vectorOriginal.xyz.

Both vectors can be the same instance to perform in-place transformation.

Parameters:

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

vectorOriginal - the original vector to use for the transformation. Not modified.

vectorTransformed - the vector used to stored the result of the transformation. Modified.

Throws:

NotAMatrix2DException - if the matrix is not a 2D matrix and checkIfTransformInXYPlane is true.

SingularMatrixException - if matrix is not invertible.

inverseTransform

public static void inverseTransform(Matrix3DReadOnly matrix,
                                    Matrix3DReadOnly matrixOriginal,
                                    Matrix3D matrixTransformed)

Undoes the transformation on matrixOriginal using matrix and stores the result in matrixTransformed:

matrixTransformed = matrix^-1 * matrixOriginal * matrix.

WARNING: This is different from concatenating orientations.

matrixOriginal and matrixTransformed can be the same instance to perform in-place transformation.

Parameters:

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

matrixOriginal - the original matrix to use for the transformation. Not modified.

matrixTransformed - the matrix used to stored the result of the transformation. Modified.

columnOutOfBoundsException

public static java.lang.ArrayIndexOutOfBoundsException columnOutOfBoundsException(int maxColumnIndex,
                                                                                  int column)

Create an ArrayIndexOutOfBoundsException for a bad column index.

Parameters:

maxColumnIndex - maximum column index allowed.

column - the bad column index.

Returns:

the exception

rowOutOfBoundsException

public static java.lang.ArrayIndexOutOfBoundsException rowOutOfBoundsException(int maxRowIndex,
                                                                               int row)

Create an ArrayIndexOutOfBoundsException for a bad row index.

Parameters:

maxRowIndex - the maximum row index allowed.

row - the bad row index.

Returns:

the exception