Rotation implements Serializable { public Vector3D applyInverseTo(Vector3D u) { double x = u.getX(); double y = u.getY(); double z = u.getZ(); double s = q1 * x + q2 * y + q3 * z; double m0 = -q0; return new Vector3D(2 * (m0 * (x * m0 - (q2 * z - q3 * y)) + s * q1) - x, 2 * (m0 * (y * m0 - (q3 * x - q1 * z)) + s * q2) - y, 2 * (m0 * (z * m0 - (q1 * y - q2 * x)) + s * q3) - z); } Rotation(double q0, double q1, double q2, double q3,
                  boolean needsNormalization);  Rotation(Vector3D axis, double angle);  Rotation(double[][] m, double threshold);  Rotation(Vector3D u1, Vector3D u2, Vector3D v1, Vector3D v2);  Rotation(Vector3D u, Vector3D v);  Rotation(RotationOrder order,
                  double alpha1, double alpha2, double alpha3); Rotation revert(); double getQ0(); double getQ1(); double getQ2(); double getQ3(); Vector3D getAxis(); double getAngle(); double[] getAngles(RotationOrder order); double[][] getMatrix(); Vector3D applyTo(Vector3D u); void applyTo(final double[] in, final double[] out); Vector3D applyInverseTo(Vector3D u); void applyInverseTo(final double[] in, final double[] out); Rotation applyTo(Rotation r); Rotation applyInverseTo(Rotation r); static double distance(Rotation r1, Rotation r2); static final Rotation IDENTITY; }
@Test public void testApplyInverseTo() throws MathIllegalArgumentException { Rotation r = new Rotation(new Vector3D(2, -3, 5), 1.7); for (double lambda = 0; lambda < 6.2; lambda += 0.2) { for (double phi = -1.55; phi < 1.55; phi += 0.2) { Vector3D u = new Vector3D(FastMath.cos(lambda) * FastMath.cos(phi), FastMath.sin(lambda) * FastMath.cos(phi), FastMath.sin(phi)); r.applyInverseTo(r.applyTo(u)); checkVector(u, r.applyInverseTo(r.applyTo(u))); checkVector(u, r.applyTo(r.applyInverseTo(u))); } } r = Rotation.IDENTITY; for (double lambda = 0; lambda < 6.2; lambda += 0.2) { for (double phi = -1.55; phi < 1.55; phi += 0.2) { Vector3D u = new Vector3D(FastMath.cos(lambda) * FastMath.cos(phi), FastMath.sin(lambda) * FastMath.cos(phi), FastMath.sin(phi)); checkVector(u, r.applyInverseTo(r.applyTo(u))); checkVector(u, r.applyTo(r.applyInverseTo(u))); } } r = new Rotation(Vector3D.PLUS_K, FastMath.PI); for (double lambda = 0; lambda < 6.2; lambda += 0.2) { for (double phi = -1.55; phi < 1.55; phi += 0.2) { Vector3D u = new Vector3D(FastMath.cos(lambda) * FastMath.cos(phi), FastMath.sin(lambda) * FastMath.cos(phi), FastMath.sin(phi)); checkVector(u, r.applyInverseTo(r.applyTo(u))); checkVector(u, r.applyTo(r.applyInverseTo(u))); } } }