/** * Compute the yaw rotation from the yaw-pitch-roll rotations induced by the given rotationMatrix. * @param rotationMatrix rotation matrix from which the yaw is computed. * @return yaw rotation (around the z-axis). */ public static double computeYaw(Matrix3d rotationMatrix) { double yaw = Math.atan2(rotationMatrix.getM10(), rotationMatrix.getM00()); if (Double.isNaN(yaw)) { throw new RuntimeException("Yaw is NaN! rotationMatrix = " + rotationMatrix); } return yaw; }
double yaw1 = Math.atan2(rotationMatrix.getM10()/cosp1, rotationMatrix.getM00()/cosp1); double roll1 = Math.atan2(rotationMatrix.getM21()/cosp1, rotationMatrix.getM22()/cosp1); double diff1 = (yaw1-yawRef)*(yaw1-yawRef) + (pitch1-pitchRef)*(pitch1-pitchRef) + (roll1-rollRef)*(roll1-rollRef); double yaw2 = Math.atan2(rotationMatrix.getM10()/cosp2, rotationMatrix.getM00()/cosp2); double roll2 = Math.atan2(rotationMatrix.getM21()/cosp2, rotationMatrix.getM22()/cosp2); double diff2 = (yaw2-yawRef)*(yaw2-yawRef) + (pitch2-pitchRef)*(pitch2-pitchRef) + (roll2-rollRef)*(roll2-rollRef); cosp = rotationMatrix.getM10() / sinc;
/** * Set the 3x3 rotation matrix equal to mat3d. * * @param matrix */ public void setRotation(Matrix3d matrix) { mat00 = matrix.getM00(); mat01 = matrix.getM01(); mat02 = matrix.getM02(); mat10 = matrix.getM10(); mat11 = matrix.getM11(); mat12 = matrix.getM12(); mat20 = matrix.getM20(); mat21 = matrix.getM21(); mat22 = matrix.getM22(); }
/** * Sets the top left 3x3 equal to matrix. * * @param matrix */ public void setRotationScale(Matrix3d matrix) { mat00 = matrix.getM00(); mat01 = matrix.getM01(); mat02 = matrix.getM02(); mat10 = matrix.getM10(); mat11 = matrix.getM11(); mat12 = matrix.getM12(); mat20 = matrix.getM20(); mat21 = matrix.getM21(); mat22 = matrix.getM22(); }
public static void computePrincipalMomentsOfInertia(Matrix3d Inertia, Matrix3d principalAxesRotationToPack, Vector3d principalMomentsOfInertiaToPack) { double[][] moiArray = new double[][]{ {Inertia.getM00(), Inertia.getM01(), Inertia.getM02()}, {Inertia.getM10(), Inertia.getM11(), Inertia.getM12()}, {Inertia.getM20(), Inertia.getM21(), Inertia.getM22()} }; Matrix inertiaForSVD = new Matrix(moiArray); computePrincipalMomentsOfInertia(inertiaForSVD, principalAxesRotationToPack, principalMomentsOfInertiaToPack); }
public void update(Matrix3d m) { update(m.getM00(), m.getM01(), m.getM02(), m.getM10(), m.getM11(), m.getM12(), m.getM20(), m.getM21(), m.getM22()); }
/** * Set the 3x3 rotation matrix equal to mat3d. Preserves scale. * * @param matrix */ @Override public void setRotation(Matrix3d matrix) { computeScale(); mat00 = scale1 * matrix.getM00(); mat01 = scale2 * matrix.getM01(); mat02 = scale3 * matrix.getM02(); mat10 = scale1 * matrix.getM10(); mat11 = scale2 * matrix.getM11(); mat12 = scale3 * matrix.getM12(); mat20 = scale1 * matrix.getM20(); mat21 = scale2 * matrix.getM21(); mat22 = scale3 * matrix.getM22(); }
/** * Set this transform to have zero translation and a rotation equal to the * Matrix3d matrix. * * @param matrix */ public final void set(Matrix3d matrix, Vector3d vector, double scale) { mat00 = scale * matrix.getM00(); mat01 = scale * matrix.getM01(); mat02 = scale * matrix.getM02(); mat10 = scale * matrix.getM10(); mat11 = scale * matrix.getM11(); mat12 = scale * matrix.getM12(); mat20 = scale * matrix.getM20(); mat21 = scale * matrix.getM21(); mat22 = scale * matrix.getM22(); setTranslation(vector.getX(),vector.getY(),vector.getZ()); }
/** * Set this transform to have zero translation and a rotation equal to the * Matrix3d matrix. * * @param matrix */ public final void set(Matrix3d matrix, Vector3d vector, double scalex, double scaley, double scalez) { mat00 = scalex * matrix.getM00(); mat01 = scaley * matrix.getM01(); mat02 = scalez * matrix.getM02(); mat10 = scalex * matrix.getM10(); mat11 = scaley * matrix.getM11(); mat12 = scalez * matrix.getM12(); mat20 = scalex * matrix.getM20(); mat21 = scaley * matrix.getM21(); mat22 = scalez * matrix.getM22(); setTranslation(vector.getX(),vector.getY(),vector.getZ()); }
/** * Verifies whether the given matrix is skew-symmetric. * * @param mat matrix to check for skew-symmetry * @param epsilon numerical tolerance */ public static void assertSkewSymmetric(Matrix3d mat, double epsilon) { // diagonal terms assertEquals(0.0, mat.getM00(), epsilon); assertEquals(0.0, mat.getM11(), epsilon); assertEquals(0.0, mat.getM22(), epsilon); // off-diagonal terms assertEquals(0.0, mat.getM01() + mat.getM10(), epsilon); assertEquals(0.0, mat.getM02() + mat.getM20(), epsilon); assertEquals(0.0, mat.getM12() + mat.getM21(), epsilon); }
public static void convertRotationMatrixToAffine(Matrix3d rotation, Affine affineToModify) { affineToModify.setMxx(rotation.getM00()); affineToModify.setMxy(rotation.getM01()); affineToModify.setMxz(rotation.getM02()); affineToModify.setMyx(rotation.getM10()); affineToModify.setMyy(rotation.getM11()); affineToModify.setMyz(rotation.getM12()); affineToModify.setMzx(rotation.getM20()); affineToModify.setMzy(rotation.getM21()); affineToModify.setMzz(rotation.getM22()); }
mx.setM00(x.getM00() * x.getM00() + x.getM10() * x.getM10() + x.getM20() * x.getM20()); mx.setM10(x.getM01() * x.getM00() + x.getM11() * x.getM10() + x.getM21() * x.getM20()); mx.setM20(x.getM02() * x.getM00() + x.getM12() * x.getM10() + x.getM22() * x.getM20()); mx.setM01(x.getM00() * x.getM01() + x.getM10() * x.getM11() + x.getM20() * x.getM21()); mx.setM11(x.getM01() * x.getM01() + x.getM11() * x.getM11() + x.getM21() * x.getM21()); mx.setM21(x.getM02() * x.getM01() + x.getM12() * x.getM11() + x.getM22() * x.getM21()); mx.setM02(x.getM00() * x.getM02() + x.getM10() * x.getM12() + x.getM20() * x.getM22()); mx.setM12(x.getM01() * x.getM02() + x.getM11() * x.getM12() + x.getM21() * x.getM22()); mx.setM22(x.getM02() * x.getM02() + x.getM12() * x.getM12() + x.getM22() * x.getM22()); rotationMatrixResultToPack.setElement(0, 0, x.getM00() - 0.5 * (x.getM00() * mx.getM00() + x.getM01() * mx.getM10() + x.getM02() * mx.getM20() - x.getM00())); rotationMatrixResultToPack.setElement(1, 0, x.getM01() - 0.5 * (x.getM00() * mx.getM01() + x.getM01() * mx.getM11() + x.getM02() * mx.getM21() - x.getM01())); rotationMatrixResultToPack.setElement(2, 0, x.getM02() - 0.5 * (x.getM00() * mx.getM02() + x.getM01() * mx.getM12() + x.getM02() * mx.getM22() - x.getM02())); rotationMatrixResultToPack.setElement(0, 1, x.getM10() - 0.5 * (x.getM10() * mx.getM00() + x.getM11() * mx.getM10() + x.getM12() * mx.getM20() - x.getM10())); rotationMatrixResultToPack.setElement(1, 1, x.getM11() - 0.5 * (x.getM10() * mx.getM01() + x.getM11() * mx.getM11() + x.getM12() * mx.getM21() - x.getM11())); rotationMatrixResultToPack.setElement(2, 1, x.getM12() - 0.5 * (x.getM10() * mx.getM02() + x.getM11() * mx.getM12() + x.getM12() * mx.getM22() - x.getM12())); rotationMatrixResultToPack.setElement(0, 2, x.getM20() - 0.5 * (x.getM20() * mx.getM00() + x.getM21() * mx.getM10() + x.getM22() * mx.getM20() - x.getM20())); rotationMatrixResultToPack.setElement(1, 2, x.getM21() - 0.5 * (x.getM20() * mx.getM01() + x.getM21() * mx.getM11() + x.getM22() * mx.getM21() - x.getM21())); rotationMatrixResultToPack.setElement(2, 2, x.getM22() - 0.5 * (x.getM20() * mx.getM02() + x.getM21() * mx.getM12() + x.getM22() * mx.getM22() - x.getM22())); corr.setM01(rotationMatrixResultToPack.getElement(1, 0) - x.getM01()); corr.setM02(rotationMatrixResultToPack.getElement(2, 0) - x.getM02()); corr.setM10(rotationMatrixResultToPack.getElement(0, 1) - x.getM10()); corr.setM11(rotationMatrixResultToPack.getElement(1, 1) - x.getM11()); corr.setM12(rotationMatrixResultToPack.getElement(2, 1) - x.getM12()); fn1 = corr.getM00() * corr.getM00() + corr.getM01() * corr.getM01() + corr.getM02() * corr.getM02() + corr.getM10() * corr.getM10() + corr.getM11() * corr.getM11() + corr.getM12() * corr.getM12() + corr.getM20() * corr.getM20() + corr.getM21() * corr.getM21() + corr.getM22() * corr.getM22();
/** * Set this transform to have zero translation and a rotation equal to the * Matrix3d matrix. * * @param matrix */ public final void set(Matrix3d matrix, Vector3d vector, Vector3d scales) { mat00 = scales.getX() * matrix.getM00(); mat01 = scales.getY() * matrix.getM01(); mat02 = scales.getZ() * matrix.getM02(); mat10 = scales.getX() * matrix.getM10(); mat11 = scales.getY() * matrix.getM11(); mat12 = scales.getZ() * matrix.getM12(); mat20 = scales.getX() * matrix.getM20(); mat21 = scales.getY() * matrix.getM21(); mat22 = scales.getZ() * matrix.getM22(); setTranslation(vector.getX(),vector.getY(),vector.getZ()); }