us.ihmc.utilities.math.geometry
Class RotationFunctions

java.lang.Object
  extended by us.ihmc.utilities.math.geometry.RotationFunctions

public class RotationFunctions
extends java.lang.Object


Field Summary
static double MATRIX_TO_QUATERNION_THRESHOLD
           
static int QUATERNION_SIZE
           
 
Constructor Summary
RotationFunctions()
           
 
Method Summary
static void assertProper(javax.vecmath.Matrix3d matrix3d)
           
static void assertProper(javax.media.j3d.Transform3D transform3D)
           
static void assertQuaternionNormalized(javax.vecmath.Quat4d q, java.lang.String errorMessage)
           
static void checkUnitLength(javax.vecmath.Quat4d orientation)
           
static ReferenceFrame findAncestorWithNonProperTransformToParent(ReferenceFrame frame)
           
static double getPitch(javax.vecmath.Matrix3d rotationMatrix)
           
static double getRoll(javax.vecmath.Matrix3d rotationMatrix)
           
static double getYaw(javax.vecmath.Matrix3d rotationMatrix)
           
static void getYawPitchRoll(double[] yawPitchRoll, javax.vecmath.Matrix3d rotationMatrix)
           
static void getYawPitchRoll(double[] yawPitchRoll, javax.media.j3d.Transform3D transform)
           
static boolean isNaNorInf(javax.media.j3d.Transform3D transformIn)
           
static boolean isQuaternionNormalized(javax.vecmath.Quat4d q)
           
static boolean isRotationProper(javax.vecmath.Matrix3d mat)
           
static boolean isRotationProper(javax.media.j3d.Transform3D transform3D)
           
static void matrixToQuaternion(javax.vecmath.Quat4d quaternion, org.ejml.data.DenseMatrix64F matrix, int rowStart)
           
static void orthogonalizeMatrix(javax.vecmath.Matrix3d matrixResultToPack, javax.vecmath.Matrix3d matrixToOrthogonalize, double threshold)
          From Apache.
static void projectRotationOntoZAxis(javax.media.j3d.Transform3D localTransform)
           
static boolean quaternionEpsilonEquals(javax.vecmath.Quat4d original, javax.vecmath.Quat4d result, double epsilon)
           
static void quaternionToMatrix(org.ejml.data.DenseMatrix64F matrix, javax.vecmath.Quat4d quaternion, int rowStart)
           
static void setAxisAngleBasedOnRotationVector(javax.vecmath.AxisAngle4d axisAngleToPack, javax.vecmath.Vector3d rotationVector)
           
static void setQuaternionBasedOnMatrix3d(javax.vecmath.Quat4d quaternionToPack, javax.vecmath.Matrix3d rotationMatrix)
          From Apache...
static void setQuaternionBasedOnTransform(javax.vecmath.Quat4d quaternionToPack, javax.media.j3d.Transform3D transform)
           
static void setQuaternionBasedOnYawPitchRoll(javax.vecmath.Quat4d q, double yaw, double pitch, double roll)
          Sets Euler parameters (unit quaternion) based on the given yaw, pitch and roll values.
static void setYawPitchRoll(javax.vecmath.Matrix3d rotationMatrixToPack, double yaw, double pitch, double roll)
          Sets the rotation matrix, based on the yaw, pitch and roll values.
static void setYawPitchRollBasedOnQuaternion(double[] yawPitchRollToPack, javax.vecmath.Quat4d q)
           
 
Methods inherited from class java.lang.Object
equals, getClass, hashCode, notify, notifyAll, toString, wait, wait, wait
 

Field Detail

QUATERNION_SIZE

public static final int QUATERNION_SIZE
See Also:
Constant Field Values

MATRIX_TO_QUATERNION_THRESHOLD

public static final double MATRIX_TO_QUATERNION_THRESHOLD
See Also:
Constant Field Values
Constructor Detail

RotationFunctions

public RotationFunctions()
Method Detail

setYawPitchRoll

public static void setYawPitchRoll(javax.vecmath.Matrix3d rotationMatrixToPack,
                                   double yaw,
                                   double pitch,
                                   double roll)
Sets the rotation matrix, based on the yaw, pitch and roll values.

Parameters:
rotationMatrixToPack - the rotation matrix to set, based on the yaw, pitch and roll values
yaw - yaw rotation (about a fixed z-axis)
pitch - pitch rotation (about a fixed y-axis)
roll - roll rotation (about a fixed x-axis)

getYaw

public static double getYaw(javax.vecmath.Matrix3d rotationMatrix)

getPitch

public static double getPitch(javax.vecmath.Matrix3d rotationMatrix)

getRoll

public static double getRoll(javax.vecmath.Matrix3d rotationMatrix)

getYawPitchRoll

public static void getYawPitchRoll(double[] yawPitchRoll,
                                   javax.vecmath.Matrix3d rotationMatrix)

getYawPitchRoll

public static void getYawPitchRoll(double[] yawPitchRoll,
                                   javax.media.j3d.Transform3D transform)

setQuaternionBasedOnYawPitchRoll

public static void setQuaternionBasedOnYawPitchRoll(javax.vecmath.Quat4d q,
                                                    double yaw,
                                                    double pitch,
                                                    double roll)
Sets Euler parameters (unit quaternion) based on the given yaw, pitch and roll values.

Parameters:
q - Quat4d to set
yaw - yaw rotation (about a fixed z-axis)
pitch - pitch rotation (about a fixed y-axis)
roll - roll rotation (about a fixed x-axis)

setYawPitchRollBasedOnQuaternion

public static void setYawPitchRollBasedOnQuaternion(double[] yawPitchRollToPack,
                                                    javax.vecmath.Quat4d q)

quaternionToMatrix

public static void quaternionToMatrix(org.ejml.data.DenseMatrix64F matrix,
                                      javax.vecmath.Quat4d quaternion,
                                      int rowStart)

matrixToQuaternion

public static void matrixToQuaternion(javax.vecmath.Quat4d quaternion,
                                      org.ejml.data.DenseMatrix64F matrix,
                                      int rowStart)

setAxisAngleBasedOnRotationVector

public static void setAxisAngleBasedOnRotationVector(javax.vecmath.AxisAngle4d axisAngleToPack,
                                                     javax.vecmath.Vector3d rotationVector)

assertQuaternionNormalized

public static void assertQuaternionNormalized(javax.vecmath.Quat4d q,
                                              java.lang.String errorMessage)

isQuaternionNormalized

public static boolean isQuaternionNormalized(javax.vecmath.Quat4d q)

assertProper

public static void assertProper(javax.media.j3d.Transform3D transform3D)

isRotationProper

public static boolean isRotationProper(javax.media.j3d.Transform3D transform3D)

assertProper

public static void assertProper(javax.vecmath.Matrix3d matrix3d)

isRotationProper

public static boolean isRotationProper(javax.vecmath.Matrix3d mat)

findAncestorWithNonProperTransformToParent

public static ReferenceFrame findAncestorWithNonProperTransformToParent(ReferenceFrame frame)

checkUnitLength

public static void checkUnitLength(javax.vecmath.Quat4d orientation)

isNaNorInf

public static boolean isNaNorInf(javax.media.j3d.Transform3D transformIn)

quaternionEpsilonEquals

public static boolean quaternionEpsilonEquals(javax.vecmath.Quat4d original,
                                              javax.vecmath.Quat4d result,
                                              double epsilon)

setQuaternionBasedOnTransform

public static void setQuaternionBasedOnTransform(javax.vecmath.Quat4d quaternionToPack,
                                                 javax.media.j3d.Transform3D transform)

setQuaternionBasedOnMatrix3d

public static void setQuaternionBasedOnMatrix3d(javax.vecmath.Quat4d quaternionToPack,
                                                javax.vecmath.Matrix3d rotationMatrix)
From Apache... Convert an orthogonal rotation matrix to a quaternion.

Parameters:
rotationMatrix - orthogonal rotation matrix

projectRotationOntoZAxis

public static void projectRotationOntoZAxis(javax.media.j3d.Transform3D localTransform)

orthogonalizeMatrix

public static void orthogonalizeMatrix(javax.vecmath.Matrix3d matrixResultToPack,
                                       javax.vecmath.Matrix3d matrixToOrthogonalize,
                                       double threshold)
                                throws org.apache.commons.math3.geometry.euclidean.threed.NotARotationMatrixException
From Apache. Perfect orthogonality on a 3X3 matrix.

Parameters:
matrixToOrthogonalize - initial matrix (not exactly orthogonal)
threshold - convergence threshold for the iterative orthogonality correction (convergence is reached when the difference between two steps of the Frobenius norm of the correction is below this threshold)
Throws:
org.apache.commons.math3.geometry.euclidean.threed.NotARotationMatrixException - if the matrix cannot be orthogonalized with the given threshold after 10 iterations