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 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); }
public void createInertiaEllipsoid(Matrix3d momentOfInertia, Vector3d comOffset, double mass, AppearanceDefinition appearance) { Vector3d principalMomentsOfInertia = new Vector3d(momentOfInertia.getM00(), momentOfInertia.getM11(), momentOfInertia.getM22()); Vector3d ellipsoidRadii = InertiaTools.getInertiaEllipsoidRadii(principalMomentsOfInertia, mass); this.translate(comOffset); this.addEllipsoid(ellipsoidRadii.getX(), ellipsoidRadii.getY(), ellipsoidRadii.getZ(), appearance); this.identity(); }
public void createInertiaEllipsoid(Matrix3d momentOfInertia, Vector3d comOffset, double mass, AppearanceDefinition appearance) { Vector3d principalMomentsOfInertia = new Vector3d(momentOfInertia.getM00(), momentOfInertia.getM11(), momentOfInertia.getM22()); Vector3d ellipsoidRadii = InertiaTools.getInertiaEllipsoidRadii(principalMomentsOfInertia, mass); this.translate(comOffset); this.addEllipsoid(ellipsoidRadii.getX(), ellipsoidRadii.getY(), ellipsoidRadii.getZ(), appearance); this.identity(); }
/** * 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.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(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()); corr.setM12(rotationMatrixResultToPack.getElement(2, 1) - x.getM12()); corr.setM20(rotationMatrixResultToPack.getElement(0, 2) - x.getM20()); 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();
@Override protected void updateTransformToParent(RigidBodyTransform transformToParent) { //TODO: Combine with RotationTools.removePitchAndRollFromTransform(). origin.getReferenceFrame().getTransformToDesiredFrame(nonZUpToWorld, worldFrame); nonZUpToWorld.getRotation(nonZUpToWorldRotation); double yAxisX = nonZUpToWorldRotation.getM01(); double yAxisY = nonZUpToWorldRotation.getM11(); yAxis.set(yAxisX, yAxisY, 0.0); yAxis.normalize(); zAxis.set(0.0, 0.0, 1.0); xAxis.cross(yAxis, zAxis); zUpToWorldRotation.setColumn(0, xAxis); zUpToWorldRotation.setColumn(1, yAxis); zUpToWorldRotation.setColumn(2, zAxis); transformToParent.setRotation(zUpToWorldRotation); originPoint3d.set(origin.getPoint()); nonZUpToWorld.transform(originPoint3d); translation.set(originPoint3d); transformToParent.setTranslation(translation); } }
@Override protected void computeRotationTranslation(Transform3d transform3D) { reshapingMatrix.setIdentity(); reshapingMatrix.setM00(reshapingMatrix.getM00() * radii.getX()); reshapingMatrix.setM11(reshapingMatrix.getM11() * radii.getY()); reshapingMatrix.setM22(reshapingMatrix.getM22() * radii.getZ()); reshapingTransform.setRotationAndZeroTranslation(reshapingMatrix); transform3D.setIdentity(); translationVector.set(x.getDoubleValue(), y.getDoubleValue(), z.getDoubleValue()); transform3D.setScale(scale); transform3D.setRotationEulerAndZeroTranslation(roll.getDoubleValue(), pitch.getDoubleValue(), yaw.getDoubleValue()); transform3D.setTranslation(translationVector); transform3D.multiply(reshapingTransform); }
/** * 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()); }
/** * 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); }
/** * 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)); }