diff --git a/src/changes/changes.xml b/src/changes/changes.xml index 211b5d4d3..4f23f92f1 100644 --- a/src/changes/changes.xml +++ b/src/changes/changes.xml @@ -52,6 +52,9 @@ If the output is not quite correct, check for invisible trailing spaces! + + New "Quaternion" class (package "o.a.c.m.complex"). + Added method to test for floating-point numbers equality with a relative tolerance (class "o.a.c.m.util.Precision"). diff --git a/src/main/java/org/apache/commons/math3/complex/Quaternion.java b/src/main/java/org/apache/commons/math3/complex/Quaternion.java new file mode 100644 index 000000000..096818c6d --- /dev/null +++ b/src/main/java/org/apache/commons/math3/complex/Quaternion.java @@ -0,0 +1,465 @@ +/* + * Licensed to the Apache Software Foundation (ASF) under one or more + * contributor license agreements. See the NOTICE file distributed with + * this work for additional information regarding copyright ownership. + * The ASF licenses this file to You under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with + * the License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +package org.apache.commons.math3.complex; + +import java.io.Serializable; +import org.apache.commons.math3.util.FastMath; +import org.apache.commons.math3.util.MathUtils; +import org.apache.commons.math3.util.Precision; +import org.apache.commons.math3.exception.DimensionMismatchException; +import org.apache.commons.math3.exception.ZeroException; +import org.apache.commons.math3.exception.util.LocalizedFormats; + +/** + * This class implements + * quaternions (Hamilton's hypercomplex numbers). + *
+ * Instance of this class are guaranteed to be immutable. + * + * @since 3.1 + * @version $Id$ + */ +public final class Quaternion implements Serializable { + /** Identity quaternion. */ + public static final Quaternion IDENTITY = new Quaternion(1, 0, 0, 0); + /** Zero quaternion. */ + public static final Quaternion ZERO = new Quaternion(0, 0, 0, 0); + /** i */ + public static final Quaternion I = new Quaternion(0, 1, 0, 0); + /** j */ + public static final Quaternion J = new Quaternion(0, 0, 1, 0); + /** k */ + public static final Quaternion K = new Quaternion(0, 0, 0, 1); + + /** Serializable version identifier. */ + private static final long serialVersionUID = 20092012L; + + /** First component (scalar part). */ + private final double q0; + /** Second component (first vector part). */ + private final double q1; + /** Third component (second vector part). */ + private final double q2; + /** Fourth component (third vector part). */ + private final double q3; + + /** + * Builds a quaternion from its components. + * + * @param a Scalar component. + * @param b First vector component. + * @param c Second vector component. + * @param d Third vector component. + */ + public Quaternion(final double a, + final double b, + final double c, + final double d) { + this.q0 = a; + this.q1 = b; + this.q2 = c; + this.q3 = d; + } + + /** + * Builds a quaternion from scalar and vector parts. + * + * @param scalar Scalar part of the quaternion. + * @param v Components of the vector part of the quaternion. + * + * @throws DimensionMismatchException if the array length is not 3. + */ + public Quaternion(final double scalar, + final double[] v) + throws DimensionMismatchException { + if (v.length != 3) { + throw new DimensionMismatchException(v.length, 3); + } + this.q0 = 0; + this.q1 = v[0]; + this.q2 = v[1]; + this.q3 = v[2]; + } + + /** + * Builds a pure quaternion from a vector (assuming that the scalar + * part is zero. + * + * @param v Components of the vector part of the pure quaternion. + */ + public Quaternion(final double[] v) { + this(0, v); + } + + /** + * Returns the conjugate quaternion of the instance. + * + * @return the conjugate quaternion + */ + public Quaternion getConjugate() { + return new Quaternion(q0, -q1, -q2, -q3); + } + + /** + * Returns the Hamilton product of two quaternions. + * + * @param q1 First quaternion. + * @param q2 Second quaternion. + * @return the product {@code q1} and {@code q2}, in that order. + */ + public static Quaternion product(final Quaternion q1, final Quaternion q2) { + // Components of the first quaternion. + final double q1a = q1.getQ0(); + final double q1b = q1.getQ1(); + final double q1c = q1.getQ2(); + final double q1d = q1.getQ3(); + + // Components of the second quaternion. + final double q2a = q2.getQ0(); + final double q2b = q2.getQ1(); + final double q2c = q2.getQ2(); + final double q2d = q2.getQ3(); + + // Components of the product. + final double w = q1a * q2a - q1b * q2b - q1c * q2c - q1d * q2d; + final double x = q1a * q2b + q1b * q2a + q1c * q2d - q1d * q2c; + final double y = q1a * q2c - q1b * q2d + q1c * q2a + q1d * q2b; + final double z = q1a * q2d + q1b * q2c - q1c * q2b + q1d * q2a; + + return new Quaternion(w, x, y, z); + } + + /** + * Returns the Hamilton product of the instance by a quaternion. + * + * @param q Quaternion. + * @return the product of this instance with {@code q}, in that order. + */ + public Quaternion multiply(final Quaternion q) { + return product(this, q); + } + + /** + * Computes the sum of two quaternions. + * + * @param q1 Quaternion. + * @param q2 Quaternion. + * @return the sum of {@code q1} and {@code q2}. + */ + public static Quaternion add(final Quaternion q1, + final Quaternion q2) { + return new Quaternion(q1.getQ0() + q2.getQ0(), + q1.getQ1() + q2.getQ1(), + q1.getQ2() + q2.getQ2(), + q1.getQ3() + q2.getQ3()); + } + + /** + * Computes the sum of the instance and another quaternion. + * + * @param q Quaternion. + * @return the sum of this instance and {@code q} + */ + public Quaternion add(final Quaternion q) { + return add(this, q); + } + + /** + * Subtracts two quaternions. + * + * @param q1 First Quaternion. + * @param q2 Second quaternion. + * @return the difference between {@code q1} and {@code q2}. + */ + public static Quaternion subtract(final Quaternion q1, + final Quaternion q2) { + return new Quaternion(q1.getQ0() - q2.getQ0(), + q1.getQ1() - q2.getQ1(), + q1.getQ2() - q2.getQ2(), + q1.getQ3() - q2.getQ3()); + } + + /** + * Subtracts a quaternion from the instance. + * + * @param q Quaternion. + * @return the difference between this instance and {@code q}. + */ + public Quaternion subtract(final Quaternion q) { + return subtract(this, q); + } + + /** + * Computes the dot-product of two quaternions. + * + * @param q1 Quaternion. + * @param q2 Quaternion. + * @return the dot product of {@code q1} and {@code q2}. + */ + public static double dotProduct(final Quaternion q1, + final Quaternion q2) { + return q1.getQ0() * q2.getQ0() + + q1.getQ1() * q2.getQ1() + + q1.getQ2() * q2.getQ2() + + q1.getQ3() * q2.getQ3(); + } + + /** + * Compute the dot-product of the instance by a quaternion. + * + * @param q Quaternion. + * @return the dot product of this instance and {@code q}. + */ + public double dotProduct(final Quaternion q) { + return dotProduct(q); + } + + /** + * Computes the norm of the quaternion. + * + * @return the norm. + */ + public double getNorm() { + return FastMath.sqrt(q0 * q0 + + q1 * q1 + + q2 * q2 + + q3 * q3); + } + + /** + * Computes the normalized quaternion (the versor of the instance). + * The norm of the quaternion must not be zero. + * + * @return a normalized quaternion. + * @throws ZeroException if the norm of the quaternion is zero. + */ + public Quaternion normalize() { + final double norm = getNorm(); + + if (norm < Precision.SAFE_MIN) { + throw new ZeroException(LocalizedFormats.NORM, norm); + } + + return new Quaternion(q0 / norm, + q1 / norm, + q2 / norm, + q3 / norm); + } + + /** + * {@inheritDoc} + */ + @Override + public boolean equals(Object other) { + if (this == other) { + return true; + } + if (other instanceof Quaternion) { + final Quaternion q = (Quaternion) other; + return q0 == q.getQ0() && + q1 == q.getQ1() && + q2 == q.getQ2() && + q3 == q.getQ3(); + } + + return false; + } + + /** + * {@inheritDoc} + */ + @Override + public int hashCode() { + // "Effective Java" (second edition, p. 47). + int result = 17; + for (double comp : new double[] { q0, q1, q2, q3 }) { + final int c = MathUtils.hash(comp); + result = 31 * result + c; + } + return result; + } + + /** + * Checks whether this instance is equal to another quaternion + * within a given tolerance. + * + * @param q Quaternion with which to compare the current quaternion. + * @param eps Tolerance. + * @return {@code true} if the each of the components are equal + * within the allowed absolute error. + */ + public boolean equals(final Quaternion q, + final double eps) { + return Precision.equals(q0, q.getQ0(), eps) && + Precision.equals(q1, q.getQ1(), eps) && + Precision.equals(q2, q.getQ2(), eps) && + Precision.equals(q3, q.getQ3(), eps); + } + + /** + * Checks whether the instance is a unit quaternion within a given + * tolerance. + * + * @param eps Tolerance (absolute error). + * @return {@code true} if the norm is 1 within the given tolerance, + * {@code false} otherwise + */ + public boolean isUnitQuaternion(double eps) { + return Precision.equals(getNorm(), 1d, eps); + } + + /** + * Checks whether the instance is a pure quaternion within a given + * tolerance. + * + * @param eps Tolerance (absolute error). + * @return {@code true} if the scalar part of the quaternion is zero. + */ + public boolean isPureQuaternion(double eps) { + return FastMath.abs(getQ0()) <= eps; + } + + /** + * Returns the polar form of the quaternion. + * + * @return the unit quaternion with positive scalar part. + */ + public Quaternion getPositivePolarForm() { + if (getQ0() < 0) { + final Quaternion unitQ = normalize(); + // The quaternion of rotation (normalized quaternion) q and -q + // are equivalent (i.e. represent the same rotation). + return new Quaternion(-unitQ.getQ0(), + -unitQ.getQ1(), + -unitQ.getQ2(), + -unitQ.getQ3()); + } else { + return this.normalize(); + } + } + + /** + * Returns the inverse of this instance. + * The norm of the quaternion must not be zero. + * + * @return the inverse. + * @throws ZeroException if the norm (squared) of the quaternion is zero. + */ + public Quaternion getInverse() { + final double squareNorm = q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3; + if (squareNorm < Precision.SAFE_MIN) { + throw new ZeroException(LocalizedFormats.NORM, squareNorm); + } + + return new Quaternion(q0 / squareNorm, + -q1 / squareNorm, + -q2 / squareNorm, + -q3 / squareNorm); + } + + /** + * Gets the first component of the quaternion (scalar part). + * + * @return the scalar part. + */ + public double getQ0() { + return q0; + } + + /** + * Gets the second component of the quaternion (first component + * of the vector part). + * + * @return the first component of the vector part. + */ + public double getQ1() { + return q1; + } + + /** + * Gets the third component of the quaternion (second component + * of the vector part). + * + * @return the second component of the vector part. + */ + public double getQ2() { + return q2; + } + + /** + * Gets the fourth component of the quaternion (third component + * of the vector part). + * + * @return the third component of the vector part. + */ + public double getQ3() { + return q3; + } + + /** + * Gets the scalar part of the quaternion. + * + * @return the scalar part. + * @see #getQ0() + */ + public double getScalarPart() { + return getQ0(); + } + + /** + * Gets the three components of the vector part of the quaternion. + * + * @return the vector part. + * @see #getQ1() + * @see #getQ2() + * @see #getQ3() + */ + public double[] getVectorPart() { + return new double[] { getQ1(), getQ2(), getQ3() }; + } + + /** + * Multiplies the instance by a scalar. + * + * @param alpha Scalar factor. + * @return a scaled quaternion. + */ + public Quaternion multiply(final double alpha) { + return new Quaternion(alpha * q0, + alpha * q1, + alpha * q2, + alpha * q3); + } + + /** + * {@inheritDoc} + */ + @Override + public String toString() { + final String sp = " "; + final StringBuilder s = new StringBuilder(); + s.append("[") + .append(q0).append(sp) + .append(q1).append(sp) + .append(q2).append(sp) + .append(q3) + .append("]"); + + return s.toString(); + } +} diff --git a/src/main/java/org/apache/commons/math3/exception/util/LocalizedFormats.java b/src/main/java/org/apache/commons/math3/exception/util/LocalizedFormats.java index 27c21fbc3..93c57e616 100644 --- a/src/main/java/org/apache/commons/math3/exception/util/LocalizedFormats.java +++ b/src/main/java/org/apache/commons/math3/exception/util/LocalizedFormats.java @@ -182,6 +182,7 @@ public enum LocalizedFormats implements Localizable { NON_REAL_FINITE_ORDINATE("all ordinatae must be finite real numbers, but {0}-th is {1}"), NON_REAL_FINITE_WEIGHT("all weights must be finite real numbers, but {0}-th is {1}"), NON_SQUARE_MATRIX("non square ({0}x{1}) matrix"), + NORM("Norm ({0})"), /* keep */ NORMALIZE_INFINITE("Cannot normalize to an infinite value"), NORMALIZE_NAN("Cannot normalize to NaN"), NOT_ADDITION_COMPATIBLE_MATRICES("{0}x{1} and {2}x{3} matrices are not addition compatible"), diff --git a/src/main/resources/assets/org/apache/commons/math3/exception/util/LocalizedFormats_fr.properties b/src/main/resources/assets/org/apache/commons/math3/exception/util/LocalizedFormats_fr.properties index 7a18b8fde..0cb7bba3c 100644 --- a/src/main/resources/assets/org/apache/commons/math3/exception/util/LocalizedFormats_fr.properties +++ b/src/main/resources/assets/org/apache/commons/math3/exception/util/LocalizedFormats_fr.properties @@ -153,6 +153,7 @@ NON_REAL_FINITE_ABSCISSA = toutes les abscisses doivent \u00eatre des nombres r\ NON_REAL_FINITE_ORDINATE = toutes les ordonn\u00e9es doivent \u00eatre des nombres r\u00e9els finis, mais l''ordonn\u00e9e {0} vaut {1} NON_REAL_FINITE_WEIGHT = tous les poids doivent \u00eatre des nombres r\u00e9els finis, mais le poids {0} vaut {1} NON_SQUARE_MATRIX = matrice non carr\u00e9e ({0}x{1}) +NORM = norme ({0}) NORMALIZE_INFINITE = impossible de normaliser vers une valeur infinie NORMALIZE_NAN = impossible de normaliser vers NaN NOT_ADDITION_COMPATIBLE_MATRICES = les dimensions {0}x{1} et {2}x{3} sont incompatibles pour l''addition matricielle diff --git a/src/test/java/org/apache/commons/math3/complex/QuaternionTest.java b/src/test/java/org/apache/commons/math3/complex/QuaternionTest.java new file mode 100644 index 000000000..78bd99db2 --- /dev/null +++ b/src/test/java/org/apache/commons/math3/complex/QuaternionTest.java @@ -0,0 +1,405 @@ +package org.apache.commons.math3.complex; + +import java.util.Random; +import org.apache.commons.math3.complex.Quaternion; +import org.apache.commons.math3.exception.DimensionMismatchException; +import org.apache.commons.math3.exception.ZeroException; +import org.apache.commons.math3.geometry.euclidean.threed.Rotation; +import org.apache.commons.math3.geometry.euclidean.threed.Vector3D; +import org.apache.commons.math3.util.Precision; +import org.apache.commons.math3.util.FastMath; +import org.junit.Test; +import org.junit.Assert; + +public class QuaternionTest { + /** Epsilon for double comparison. */ + private static final double EPS = Math.ulp(1d); + /** Epsilon for double comparison. */ + private static final double COMPARISON_EPS = 1e-14; + + @Test + public final void testAccessors1() { + final double q0 = 2; + final double q1 = 5.4; + final double q2 = 17; + final double q3 = 0.0005; + final Quaternion q = new Quaternion(q0, q1, q2, q3); + + Assert.assertEquals(q0, q.getQ0(), 0); + Assert.assertEquals(q1, q.getQ1(), 0); + Assert.assertEquals(q2, q.getQ2(), 0); + Assert.assertEquals(q3, q.getQ3(), 0); + } + + @Test + public final void testAccessors2() { + final double q0 = 2; + final double q1 = 5.4; + final double q2 = 17; + final double q3 = 0.0005; + final Quaternion q = new Quaternion(q0, q1, q2, q3); + + final double sP = q.getScalarPart(); + final double[] vP = q.getVectorPart(); + + Assert.assertEquals(q0, sP, 0); + Assert.assertEquals(q1, vP[0], 0); + Assert.assertEquals(q2, vP[1], 0); + Assert.assertEquals(q3, vP[2], 0); + } + + @Test(expected=DimensionMismatchException.class) + public void testWrongDimension() { + new Quaternion(new double[] { 1, 2 }); + } + + @Test + public final void testConjugate() { + final double q0 = 2; + final double q1 = 5.4; + final double q2 = 17; + final double q3 = 0.0005; + final Quaternion q = new Quaternion(q0, q1, q2, q3); + + final Quaternion qConjugate = q.getConjugate(); + + Assert.assertEquals(q0, qConjugate.getQ0(), 0); + Assert.assertEquals(-q1, qConjugate.getQ1(), 0); + Assert.assertEquals(-q2, qConjugate.getQ2(), 0); + Assert.assertEquals(-q3, qConjugate.getQ3(), 0); + } + + @Test + public final void testProductQuaternionQuaternion() { + + // Case : analytic test case + + final Quaternion qA = new Quaternion(1, 0.5, -3, 4); + final Quaternion qB = new Quaternion(6, 2, 1, -9); + final Quaternion qResult = Quaternion.product(qA, qB); + + Assert.assertEquals(44, qResult.getQ0(), EPS); + Assert.assertEquals(28, qResult.getQ1(), EPS); + Assert.assertEquals(-4.5, qResult.getQ2(), EPS); + Assert.assertEquals(21.5, qResult.getQ3(), EPS); + + // comparison with the result given by the formula : + // qResult = (scalarA * scalarB - vectorA . vectorB) + (scalarA * vectorB + scalarB * vectorA + vectorA ^ + // vectorB) + + final Vector3D vectorA = new Vector3D(qA.getVectorPart()); + final Vector3D vectorB = new Vector3D(qB.getVectorPart()); + final Vector3D vectorResult = new Vector3D(qResult.getVectorPart()); + + final double scalarPartRef = qA.getScalarPart() * qB.getScalarPart() - Vector3D.dotProduct(vectorA, vectorB); + + Assert.assertEquals(scalarPartRef, qResult.getScalarPart(), EPS); + + final Vector3D vectorPartRef = ((vectorA.scalarMultiply(qB.getScalarPart())).add(vectorB.scalarMultiply(qA + .getScalarPart()))).add(Vector3D.crossProduct(vectorA, vectorB)); + final double norm = (vectorResult.subtract(vectorPartRef)).getNorm(); + + Assert.assertEquals(0, norm, EPS); + + // Conjugate of the product of two quaternions and product of their conjugates : + // Conj(qA * qB) = Conj(qB) * Conj(qA) + + final Quaternion conjugateOfProduct = Quaternion.product(qB.getConjugate(), qA.getConjugate()); + final Quaternion productOfConjugate = (Quaternion.product(qA, qB)).getConjugate(); + + Assert.assertEquals(conjugateOfProduct.getQ0(), productOfConjugate.getQ0(), EPS); + Assert.assertEquals(conjugateOfProduct.getQ1(), productOfConjugate.getQ1(), EPS); + Assert.assertEquals(conjugateOfProduct.getQ2(), productOfConjugate.getQ2(), EPS); + Assert.assertEquals(conjugateOfProduct.getQ3(), productOfConjugate.getQ3(), EPS); + } + + @Test + public final void testProductQuaternionVector() { + + // Case : Product between a vector and a quaternion : QxV + + final Quaternion quaternion = new Quaternion(4, 7, -1, 2); + final double[] vector = {2.0, 1.0, 3.0}; + final Quaternion qResultQxV = Quaternion.product(quaternion, new Quaternion(vector)); + + Assert.assertEquals(-19, qResultQxV.getQ0(), EPS); + Assert.assertEquals(3, qResultQxV.getQ1(), EPS); + Assert.assertEquals(-13, qResultQxV.getQ2(), EPS); + Assert.assertEquals(21, qResultQxV.getQ3(), EPS); + + // comparison with the result given by the formula : + // qResult = (- vectorQ . vector) + (scalarQ * vector + vectorQ ^ vector) + + final double[] vectorQ = quaternion.getVectorPart(); + final double[] vectorResultQxV = qResultQxV.getVectorPart(); + + final double scalarPartRefQxV = -Vector3D.dotProduct(new Vector3D(vectorQ), new Vector3D(vector)); + Assert.assertEquals(scalarPartRefQxV, qResultQxV.getScalarPart(), EPS); + + final Vector3D vectorPartRefQxV = (new Vector3D(vector).scalarMultiply(quaternion.getScalarPart())).add(Vector3D + .crossProduct(new Vector3D(vectorQ), new Vector3D(vector))); + final double normQxV = (new Vector3D(vectorResultQxV).subtract(vectorPartRefQxV)).getNorm(); + Assert.assertEquals(0, normQxV, EPS); + + // Case : Product between a vector and a quaternion : VxQ + + final Quaternion qResultVxQ = Quaternion.product(new Quaternion(vector), quaternion); + + Assert.assertEquals(-19, qResultVxQ.getQ0(), EPS); + Assert.assertEquals(13, qResultVxQ.getQ1(), EPS); + Assert.assertEquals(21, qResultVxQ.getQ2(), EPS); + Assert.assertEquals(3, qResultVxQ.getQ3(), EPS); + + final double[] vectorResultVxQ = qResultVxQ.getVectorPart(); + + // comparison with the result given by the formula : + // qResult = (- vector . vectorQ) + (scalarQ * vector + vector ^ vectorQ) + + final double scalarPartRefVxQ = -Vector3D.dotProduct(new Vector3D(vectorQ), new Vector3D(vector)); + Assert.assertEquals(scalarPartRefVxQ, qResultVxQ.getScalarPart(), EPS); + + final Vector3D vectorPartRefVxQ = (new Vector3D(vector).scalarMultiply(quaternion.getScalarPart())).add(Vector3D + .crossProduct(new Vector3D(vector), new Vector3D(vectorQ))); + final double normVxQ = (new Vector3D(vectorResultVxQ).subtract(vectorPartRefVxQ)).getNorm(); + Assert.assertEquals(0, normVxQ, EPS); + } + + @Test + public final void testDotProductQuaternionQuaternion() { + // expected output + final double expected = -6.; + // inputs + final Quaternion q1 = new Quaternion(1, 2, 2, 1); + final Quaternion q2 = new Quaternion(3, -2, -1, -3); + + final double actual = Quaternion.dotProduct(q1, q2); + + Assert.assertEquals(expected, actual, EPS); + } + + @Test + public final void testScalarMultiplyDouble() { + // expected outputs + final double w = 1.6; + final double x = -4.8; + final double y = 11.20; + final double z = 2.56; + // inputs + final Quaternion q1 = new Quaternion(0.5, -1.5, 3.5, 0.8); + final double a = 3.2; + + final Quaternion q = q1.multiply(a); + + Assert.assertEquals(w, q.getQ0(), COMPARISON_EPS); + Assert.assertEquals(x, q.getQ1(), COMPARISON_EPS); + Assert.assertEquals(y, q.getQ2(), COMPARISON_EPS); + Assert.assertEquals(z, q.getQ3(), COMPARISON_EPS); + } + + @Test + public final void testAddQuaternionQuaternion() { + // expected outputs + final double w = 4; + final double x = -1; + final double y = 2; + final double z = -4; + // inputs + final Quaternion q1 = new Quaternion(1., 2., -2., -1.); + final Quaternion q2 = new Quaternion(3., -3., 4., -3.); + + final Quaternion q = Quaternion.add(q1, q2); + + Assert.assertEquals(w, q.getQ0(), EPS); + Assert.assertEquals(x, q.getQ1(), EPS); + Assert.assertEquals(y, q.getQ2(), EPS); + Assert.assertEquals(z, q.getQ3(), EPS); + } + + @Test + public final void testSubtractQuaternionQuaternion() { + // expected outputs + final double w = -2.; + final double x = 5.; + final double y = -6.; + final double z = 2.; + // inputs + final Quaternion q1 = new Quaternion(1., 2., -2., -1.); + final Quaternion q2 = new Quaternion(3., -3., 4., -3.); + + final Quaternion q = Quaternion.subtract(q1, q2); + + Assert.assertEquals(w, q.getQ0(), EPS); + Assert.assertEquals(x, q.getQ1(), EPS); + Assert.assertEquals(y, q.getQ2(), EPS); + Assert.assertEquals(z, q.getQ3(), EPS); + } + + @Test + public final void testNorm() { + + final double q0 = 2; + final double q1 = 1; + final double q2 = -4; + final double q3 = 3; + final Quaternion q = new Quaternion(q0, q1, q2, q3); + + final double norm = q.getNorm(); + + Assert.assertEquals(Math.sqrt(30), norm, 0); + + final double normSquareRef = Quaternion.product(q, q.getConjugate()).getScalarPart(); + Assert.assertEquals(Math.sqrt(normSquareRef), norm, 0); + } + + @Test + public final void testNormalize() { + + final Quaternion q = new Quaternion(2, 1, -4, -2); + + final Quaternion versor = q.normalize(); + + Assert.assertEquals(2.0 / 5.0, versor.getQ0(), 0); + Assert.assertEquals(1.0 / 5.0, versor.getQ1(), 0); + Assert.assertEquals(-4.0 / 5.0, versor.getQ2(), 0); + Assert.assertEquals(-2.0 / 5.0, versor.getQ3(), 0); + + Assert.assertEquals(1, versor.getNorm(), 0); + } + + @Test(expected=ZeroException.class) + public final void testNormalizeFail() { + final Quaternion zeroQ = new Quaternion(0, 0, 0, 0); + zeroQ.normalize(); + } + + @Test + public final void testObjectEquals() { + final double one = 1; + final Quaternion q1 = new Quaternion(one, one, one, one); + Assert.assertTrue(q1.equals(q1)); + + final Quaternion q2 = new Quaternion(one, one, one, one); + Assert.assertTrue(q2.equals(q1)); + + final Quaternion q3 = new Quaternion(one, FastMath.nextUp(one), one, one); + Assert.assertFalse(q3.equals(q1)); + } + + @Test + public final void testQuaternionEquals() { + final double inc = 1e-5; + final Quaternion q1 = new Quaternion(2, 1, -4, -2); + final Quaternion q2 = new Quaternion(q1.getQ0() + inc, q1.getQ1(), q1.getQ2(), q1.getQ3()); + final Quaternion q3 = new Quaternion(q1.getQ0(), q1.getQ1() + inc, q1.getQ2(), q1.getQ3()); + final Quaternion q4 = new Quaternion(q1.getQ0(), q1.getQ1(), q1.getQ2() + inc, q1.getQ3()); + final Quaternion q5 = new Quaternion(q1.getQ0(), q1.getQ1(), q1.getQ2(), q1.getQ3() + inc); + + Assert.assertFalse(q1.equals(q2, 0.9 * inc)); + Assert.assertFalse(q1.equals(q3, 0.9 * inc)); + Assert.assertFalse(q1.equals(q4, 0.9 * inc)); + Assert.assertFalse(q1.equals(q5, 0.9 * inc)); + + Assert.assertTrue(q1.equals(q2, 1.1 * inc)); + Assert.assertTrue(q1.equals(q3, 1.1 * inc)); + Assert.assertTrue(q1.equals(q4, 1.1 * inc)); + Assert.assertTrue(q1.equals(q5, 1.1 * inc)); + } + + @Test + public final void testQuaternionEquals2() { + final Quaternion q1 = new Quaternion(1, 4, 2, 3); + final double gap = 1e-5; + final Quaternion q2 = new Quaternion(1 + gap, 4 + gap, 2 + gap, 3 + gap); + + Assert.assertTrue(q1.equals(q2, 10 * gap)); + Assert.assertFalse(q1.equals(q2, gap)); + Assert.assertFalse(q1.equals(q2, gap / 10)); + } + + @Test + public final void testIsUnitQuaternion() { + final Random r = new Random(48); + final int numberOfTrials = 1000; + for (int i = 0; i < numberOfTrials; i++) { + final Quaternion q1 = new Quaternion(r.nextDouble(), r.nextDouble(), r.nextDouble(), r.nextDouble()); + final Quaternion q2 = q1.normalize(); + Assert.assertTrue(q2.isUnitQuaternion(COMPARISON_EPS)); + } + + final Quaternion q = new Quaternion(1, 1, 1, 1); + Assert.assertFalse(q.isUnitQuaternion(COMPARISON_EPS)); + } + + @Test + public final void testIsPureQuaternion() { + final Quaternion q1 = new Quaternion(0, 5, 4, 8); + Assert.assertTrue(q1.isPureQuaternion(EPS)); + + final Quaternion q2 = new Quaternion(0 - EPS, 5, 4, 8); + Assert.assertTrue(q2.isPureQuaternion(EPS)); + + final Quaternion q3 = new Quaternion(0 - 1.1 * EPS, 5, 4, 8); + Assert.assertFalse(q3.isPureQuaternion(EPS)); + + final Random r = new Random(48); + final double[] v = {r.nextDouble(), r.nextDouble(), r.nextDouble()}; + final Quaternion q4 = new Quaternion(v); + Assert.assertTrue(q4.isPureQuaternion(0)); + + final Quaternion q5 = new Quaternion(0, v); + Assert.assertTrue(q5.isPureQuaternion(0)); + } + + @Test + public final void testPolarForm() { + final Random r = new Random(48); + final int numberOfTrials = 1000; + for (int i = 0; i < numberOfTrials; i++) { + final Quaternion q = new Quaternion(2 * (r.nextDouble() - 0.5), 2 * (r.nextDouble() - 0.5), + 2 * (r.nextDouble() - 0.5), 2 * (r.nextDouble() - 0.5)); + final Quaternion qP = q.getPositivePolarForm(); + + Assert.assertTrue(qP.isUnitQuaternion(COMPARISON_EPS)); + Assert.assertTrue(qP.getQ0() >= 0); + + final Rotation rot = new Rotation(q.getQ0(), q.getQ1(), q.getQ2(), q.getQ3(), true); + final Rotation rotP = new Rotation(qP.getQ0(), qP.getQ1(), qP.getQ2(), qP.getQ3(), true); + + Assert.assertEquals(rot.getAngle(), rotP.getAngle(), COMPARISON_EPS); + Assert.assertEquals(rot.getAxis().getX(), rot.getAxis().getX(), COMPARISON_EPS); + Assert.assertEquals(rot.getAxis().getY(), rot.getAxis().getY(), COMPARISON_EPS); + Assert.assertEquals(rot.getAxis().getZ(), rot.getAxis().getZ(), COMPARISON_EPS); + } + } + + @Test + public final void testGetInverse() { + final Quaternion q = new Quaternion(1.5, 4, 2, -2.5); + + final Quaternion inverseQ = q.getInverse(); + Assert.assertEquals(1.5 / 28.5, inverseQ.getQ0(), 0); + Assert.assertEquals(-4.0 / 28.5, inverseQ.getQ1(), 0); + Assert.assertEquals(-2.0 / 28.5, inverseQ.getQ2(), 0); + Assert.assertEquals(2.5 / 28.5, inverseQ.getQ3(), 0); + + final Quaternion product = Quaternion.product(inverseQ, q); + Assert.assertEquals(1, product.getQ0(), EPS); + Assert.assertEquals(0, product.getQ1(), EPS); + Assert.assertEquals(0, product.getQ2(), EPS); + Assert.assertEquals(0, product.getQ3(), EPS); + + final Quaternion qNul = new Quaternion(0, 0, 0, 0); + try { + final Quaternion inverseQNul = qNul.getInverse(); + Assert.fail("expecting ZeroException but got : " + inverseQNul); + } catch (ZeroException ex) { + // expected + } + } + + @Test + public final void testToString() { + final Quaternion q = new Quaternion(1, 2, 3, 4); + Assert.assertTrue(q.toString().equals("[1.0 2.0 3.0 4.0]")); + } +} diff --git a/src/test/java/org/apache/commons/math3/exception/util/LocalizedFormatsTest.java b/src/test/java/org/apache/commons/math3/exception/util/LocalizedFormatsTest.java index 22a0a5288..8f8928ebe 100644 --- a/src/test/java/org/apache/commons/math3/exception/util/LocalizedFormatsTest.java +++ b/src/test/java/org/apache/commons/math3/exception/util/LocalizedFormatsTest.java @@ -36,7 +36,7 @@ public class LocalizedFormatsTest { @Test public void testMessageNumber() { - Assert.assertEquals(310, LocalizedFormats.values().length); + Assert.assertEquals(311, LocalizedFormats.values().length); } @Test