public static boolean isRotationInPlane(Matrix3d rotation) { // true if the transform preserves the z axis direction and magnitude, paying no heed to transformations. // for SO(3) matrices (orthogonal with determinant 1, equivalent to rotations) RotationTools.checkProperRotationMatrix(rotation); double epsilon = 1e-14; return ((Math.abs(rotation.getM02()) <= epsilon) && (Math.abs(rotation.getM12()) <= epsilon) && (Math.abs(rotation.getM22() - 1.0) <= epsilon)); }
private SDFInertia createSDFInertia(Link scsLink) { SDFInertia sdfInertia = new SDFInertia(); Matrix3d scsInertia = new Matrix3d(); scsLink.getMomentOfInertia(scsInertia); sdfInertia.setIxx(String.valueOf(scsInertia.getM00())); sdfInertia.setIyy(String.valueOf(scsInertia.getM11())); sdfInertia.setIzz(String.valueOf(scsInertia.getM22())); sdfInertia.setIxy(String.valueOf(scsInertia.getM01())); sdfInertia.setIxz(String.valueOf(scsInertia.getM02())); sdfInertia.setIyz(String.valueOf(scsInertia.getM12())); return sdfInertia; }
double m02 = rotationMatrix.getM02(); double m11 = rotationMatrix.getM11(); double m12 = rotationMatrix.getM12();
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. * * @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); }
/** * 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()); }
/** * packs M with M - scalar * (tilde(a) * tilde(a)) * * @param M a symmetric matrix - NOTE: no symmetry checks are being performed * @param a a vector * @param scalar a scalar */ private static void subScalarTimesTildeTimesTildeFromSymmetricMatrix(Matrix3d M, Vector3d a, double scalar) { double xSquared = scalar * a.getX() * a.getX(); double ySquared = scalar * a.getY() * a.getY(); double zSquared = scalar * a.getZ() * a.getZ(); double xy = scalar * a.getX() * a.getY(); double xz = scalar * a.getX() * a.getZ(); double yz = scalar * a.getY() * a.getZ(); M.setM00(M.getM00() + ySquared + zSquared); M.setM01(M.getM01() - xy); M.setM02(M.getM02() - xz); M.setM10(M.getM01()); M.setM11(M.getM11() + xSquared + zSquared); M.setM12(M.getM12() - yz); M.setM20(M.getM02()); M.setM21(M.getM12()); M.setM22(M.getM22() + xSquared + ySquared); }
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.setM02(rotationMatrixResultToPack.getElement(2, 0) - x.getM02()); corr.setM10(rotationMatrixResultToPack.getElement(0, 1) - x.getM10()); corr.setM11(rotationMatrixResultToPack.getElement(1, 1) - x.getM11()); 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();
/** * packs M with M - tilde(a) * tilde(b) - (tilde(a) * tilde(b))^T * * @param M a symmetric matrix - NOTE: no symmetry checks are being performed * @param a a vector * @param b another vector */ private static void subTildeTimesTildePlusTildeTimesTildeTransposeFromSymmetricMatrix(Matrix3d M, Vector3d a, Vector3d b) { double axbx = a.getX() * b.getX(); double ayby = a.getY() * b.getY(); double azbz = a.getZ() * b.getZ(); M.setM00(M.getM00() + 2.0 * (azbz + ayby)); M.setM01(M.getM01() - a.getY() * b.getX() - a.getX() * b.getY()); M.setM02(M.getM02() - a.getZ() * b.getX() - a.getX() * b.getZ()); M.setM10(M.getM01()); M.setM11(M.getM11() + 2.0 * (axbx + azbz)); M.setM12(M.getM12() - a.getZ() * b.getY() - a.getY() * b.getZ()); M.setM20(M.getM02()); M.setM21(M.getM12()); M.setM22(M.getM22() + 2.0 * (axbx + ayby)); }
/** * 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()); }