/** * Return rotation matrix of type Matrix3d * * @param matrix */ public void getRotation(Matrix3d matrix) { matrix.setM00(mat00); matrix.setM01(mat01); matrix.setM02(mat02); matrix.setM10(mat10); matrix.setM11(mat11); matrix.setM12(mat12); matrix.setM20(mat20); matrix.setM21(mat21); matrix.setM22(mat22); }
/** * Return scaled rotation matrix of type Matrix3d * * @param matrix */ public final void getRotationScale(Matrix3d matrix) { matrix.setM00(mat00); matrix.setM01(mat01); matrix.setM02(mat02); matrix.setM10(mat10); matrix.setM11(mat11); matrix.setM12(mat12); matrix.setM20(mat20); matrix.setM21(mat21); matrix.setM22(mat22); }
/** * Return rotation matrix of type Matrix3d * * @param matrix */ @Override public final void getRotation(Matrix3d matrix) { computeRotationScale(); matrix.setM00(rot00); matrix.setM01(rot01); matrix.setM02(rot02); matrix.setM10(rot10); matrix.setM11(rot11); matrix.setM12(rot12); matrix.setM20(rot20); matrix.setM21(rot21); matrix.setM22(rot22); }
public void update(double m00, double m01, double m02, double m10, double m11, double m12, double m20, double m21, double m22) { perfectRotationMatrix.setM00(m00); perfectRotationMatrix.setM01(m01); perfectRotationMatrix.setM02(m02); perfectRotationMatrix.setM10(m10); perfectRotationMatrix.setM11(m11); perfectRotationMatrix.setM12(m12); perfectRotationMatrix.setM20(m20); perfectRotationMatrix.setM21(m21); perfectRotationMatrix.setM22(m22); generateNoise(); generateBias(); noisyRotationMatrix.mul(perfectRotationMatrix, biasMatrix); // NoiseMatrix is transformation from IMU measurement to perfect body orientation noisyRotationMatrix.mul(noisyRotationMatrix, noiseMatrix); // NoiseMatrix is transformation from IMU measurement to perfect body orientation if (((Double)(noisyRotationMatrix.getM00())).isNaN()) { noisyRotationMatrix.set(perfectRotationMatrix); } }
public static void convertTransformToRotationMatrix(Transform transform, Matrix3d rotationToPack) { rotationToPack.setM00(transform.getMxx()); rotationToPack.setM01(transform.getMxy()); rotationToPack.setM02(transform.getMxz()); rotationToPack.setM10(transform.getMyx()); rotationToPack.setM11(transform.getMyy()); rotationToPack.setM12(transform.getMyz()); rotationToPack.setM20(transform.getMzx()); rotationToPack.setM21(transform.getMzy()); rotationToPack.setM22(transform.getMzz()); }
public static void setTildeTimesTilde(Matrix3d M, Tuple3d a, Tuple3d b) { double axbx = a.getX() * b.getX(); double ayby = a.getY() * b.getY(); double azbz = a.getZ() * b.getZ(); M.setM00(-azbz - ayby); M.setM01(a.getY() * b.getX()); M.setM02(a.getZ() * b.getX()); M.setM10(a.getX() * b.getY()); M.setM11(-axbx - azbz); M.setM12(a.getZ() * b.getY()); M.setM20(a.getX() * b.getZ()); M.setM21(a.getY() * b.getZ()); M.setM22(-axbx - ayby); }
/** * 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)); }
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()); corr.setM02(rotationMatrixResultToPack.getElement(2, 0) - x.getM02()); corr.setM10(rotationMatrixResultToPack.getElement(0, 1) - x.getM10()); corr.setM11(rotationMatrixResultToPack.getElement(1, 1) - x.getM11()); x.setM02(rotationMatrixResultToPack.getElement(2, 0)); x.setM10(rotationMatrixResultToPack.getElement(0, 1)); x.setM11(rotationMatrixResultToPack.getElement(1, 1));