MATH-1416: Remove Quaternion from commons-math as it have been moved to commons-numbers
Project: http://git-wip-us.apache.org/repos/asf/commons-math/repo Commit: http://git-wip-us.apache.org/repos/asf/commons-math/commit/e415b2f4 Tree: http://git-wip-us.apache.org/repos/asf/commons-math/tree/e415b2f4 Diff: http://git-wip-us.apache.org/repos/asf/commons-math/diff/e415b2f4 Branch: refs/heads/feature-MATH-1416 Commit: e415b2f4f3f5f4d103c249009cd313c6be011946 Parents: 63bb89e Author: Ray DeCampo <[email protected]> Authored: Sat Sep 16 11:36:10 2017 -0400 Committer: Ray DeCampo <[email protected]> Committed: Sat Sep 16 11:36:10 2017 -0400 ---------------------------------------------------------------------- .../commons/math4/complex/Quaternion.java | 465 ------------------- .../commons/math4/complex/QuaternionTest.java | 459 ------------------ 2 files changed, 924 deletions(-) ---------------------------------------------------------------------- http://git-wip-us.apache.org/repos/asf/commons-math/blob/e415b2f4/src/main/java/org/apache/commons/math4/complex/Quaternion.java ---------------------------------------------------------------------- diff --git a/src/main/java/org/apache/commons/math4/complex/Quaternion.java b/src/main/java/org/apache/commons/math4/complex/Quaternion.java deleted file mode 100644 index f5f22cc..0000000 --- a/src/main/java/org/apache/commons/math4/complex/Quaternion.java +++ /dev/null @@ -1,465 +0,0 @@ -/* - * 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.math4.complex; - -import java.io.Serializable; - -import org.apache.commons.math4.exception.DimensionMismatchException; -import org.apache.commons.math4.exception.ZeroException; -import org.apache.commons.math4.exception.util.LocalizedFormats; -import org.apache.commons.math4.util.FastMath; -import org.apache.commons.math4.util.MathUtils; -import org.apache.commons.numbers.core.Precision; - -/** - * This class implements <a href="http://mathworld.wolfram.com/Quaternion.html";> - * quaternions</a> (Hamilton's hypercomplex numbers). - * <br> - * Instance of this class are guaranteed to be immutable. - * - * @since 3.1 - */ -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 = scalar; - 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 multiply(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 multiply(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(); - } - - /** - * Computes 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(this, 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(); - } -} http://git-wip-us.apache.org/repos/asf/commons-math/blob/e415b2f4/src/test/java/org/apache/commons/math4/complex/QuaternionTest.java ---------------------------------------------------------------------- diff --git a/src/test/java/org/apache/commons/math4/complex/QuaternionTest.java b/src/test/java/org/apache/commons/math4/complex/QuaternionTest.java deleted file mode 100644 index 06feaa5..0000000 --- a/src/test/java/org/apache/commons/math4/complex/QuaternionTest.java +++ /dev/null @@ -1,459 +0,0 @@ -/* - * 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.math4.complex; - -import java.util.Random; - -import org.apache.commons.math4.complex.Quaternion; -import org.apache.commons.math4.exception.DimensionMismatchException; -import org.apache.commons.math4.exception.ZeroException; -import org.apache.commons.math4.geometry.euclidean.threed.Rotation; -import org.apache.commons.math4.geometry.euclidean.threed.RotationConvention; -import org.apache.commons.math4.geometry.euclidean.threed.Cartesian3D; -import org.apache.commons.math4.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 - public final void testAccessors3() { - final double q0 = 2; - final double q1 = 5.4; - final double q2 = 17; - final double q3 = 0.0005; - final Quaternion q = new Quaternion(q0, new double[] { 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.multiply(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 Cartesian3D vectorA = new Cartesian3D(qA.getVectorPart()); - final Cartesian3D vectorB = new Cartesian3D(qB.getVectorPart()); - final Cartesian3D vectorResult = new Cartesian3D(qResult.getVectorPart()); - - final double scalarPartRef = qA.getScalarPart() * qB.getScalarPart() - Cartesian3D.dotProduct(vectorA, vectorB); - - Assert.assertEquals(scalarPartRef, qResult.getScalarPart(), EPS); - - final Cartesian3D vectorPartRef = ((vectorA.scalarMultiply(qB.getScalarPart())).add(vectorB.scalarMultiply(qA - .getScalarPart()))).add(Cartesian3D.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 = qB.getConjugate().multiply(qA.getConjugate()); - final Quaternion productOfConjugate = (qA.multiply(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.multiply(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 = -Cartesian3D.dotProduct(new Cartesian3D(vectorQ), new Cartesian3D(vector)); - Assert.assertEquals(scalarPartRefQxV, qResultQxV.getScalarPart(), EPS); - - final Cartesian3D vectorPartRefQxV = (new Cartesian3D(vector).scalarMultiply(quaternion.getScalarPart())).add(Cartesian3D - .crossProduct(new Cartesian3D(vectorQ), new Cartesian3D(vector))); - final double normQxV = (new Cartesian3D(vectorResultQxV).subtract(vectorPartRefQxV)).getNorm(); - Assert.assertEquals(0, normQxV, EPS); - - // Case : Product between a vector and a quaternion : VxQ - - final Quaternion qResultVxQ = Quaternion.multiply(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 = -Cartesian3D.dotProduct(new Cartesian3D(vectorQ), new Cartesian3D(vector)); - Assert.assertEquals(scalarPartRefVxQ, qResultVxQ.getScalarPart(), EPS); - - final Cartesian3D vectorPartRefVxQ = (new Cartesian3D(vector).scalarMultiply(quaternion.getScalarPart())).add(Cartesian3D - .crossProduct(new Cartesian3D(vector), new Cartesian3D(vectorQ))); - final double normVxQ = (new Cartesian3D(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 actual1 = Quaternion.dotProduct(q1, q2); - final double actual2 = q1.dotProduct(q2); - - Assert.assertEquals(expected, actual1, EPS); - Assert.assertEquals(expected, actual2, 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 qa = Quaternion.add(q1, q2); - final Quaternion qb = q1.add(q2); - - Assert.assertEquals(w, qa.getQ0(), EPS); - Assert.assertEquals(x, qa.getQ1(), EPS); - Assert.assertEquals(y, qa.getQ2(), EPS); - Assert.assertEquals(z, qa.getQ3(), EPS); - - Assert.assertEquals(w, qb.getQ0(), EPS); - Assert.assertEquals(x, qb.getQ1(), EPS); - Assert.assertEquals(y, qb.getQ2(), EPS); - Assert.assertEquals(z, qb.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 qa = Quaternion.subtract(q1, q2); - final Quaternion qb = q1.subtract(q2); - - Assert.assertEquals(w, qa.getQ0(), EPS); - Assert.assertEquals(x, qa.getQ1(), EPS); - Assert.assertEquals(y, qa.getQ2(), EPS); - Assert.assertEquals(z, qa.getQ3(), EPS); - - Assert.assertEquals(w, qb.getQ0(), EPS); - Assert.assertEquals(x, qb.getQ1(), EPS); - Assert.assertEquals(y, qb.getQ2(), EPS); - Assert.assertEquals(z, qb.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(FastMath.sqrt(30), norm, 0); - - final double normSquareRef = Quaternion.multiply(q, q.getConjugate()).getScalarPart(); - Assert.assertEquals(FastMath.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(RotationConvention.VECTOR_OPERATOR).getX(), - rot.getAxis(RotationConvention.VECTOR_OPERATOR).getX(), - COMPARISON_EPS); - Assert.assertEquals(rot.getAxis(RotationConvention.VECTOR_OPERATOR).getY(), - rot.getAxis(RotationConvention.VECTOR_OPERATOR).getY(), - COMPARISON_EPS); - Assert.assertEquals(rot.getAxis(RotationConvention.VECTOR_OPERATOR).getZ(), - rot.getAxis(RotationConvention.VECTOR_OPERATOR).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.multiply(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]")); - } -}
