MATH-863
New "Quaternion" class. Thanks to Julien Anxionnat.
After applying the original patch, the following changes were made:
* Renamed the "static" quaternion instances ("PLUS_" prefix removed).
* Removed some (syntactic sugar) methods; removed or modified corresponding
unit tests.
* Made the redundant accessors call the "canonic" ones.
* Removed the default tolerance and added an explicit tolerance parameter
in methods that depend on equality testing.
* When a "ZeroException" is thrown, the actual value of the norm is provided
in the detailed message (as the implementation actually does not use a
strict comparison with 0).
* Added "equals(Object)" and "hashCode" methods.
* Javadoc and formatting. Added license header.
* Removed "toString" documentation (as this representation should not be
binding). Changed the representation to not use a comma.
* Renamed "scalarMultiply" to "multiply".
* More stringent tolerance used in the unit tests assertions, whenever
possible.
* Added unit tests.
git-svn-id: https://svn.apache.org/repos/asf/commons/proper/math/trunk@1388099 13f79535-47bb-0310-9956-ffa450edef68
This commit is contained in:
Gilles Sadowski
parent
600f00ba58
commit
7a74137904
|
|
@ -52,6 +52,9 @@ If the output is not quite correct, check for invisible trailing spaces!
|
|||
<body>
|
||||
<release version="3.1" date="TBD" description="
|
||||
">
|
||||
<action dev="erans" type="add" issue="MATH-863" due-to="Julien Anxionnat">
|
||||
New "Quaternion" class (package "o.a.c.m.complex").
|
||||
</action>
|
||||
<action dev="erans" type="add" issue="MATH-866" due-to="Yannick Tanguy">
|
||||
Added method to test for floating-point numbers equality with a
|
||||
relative tolerance (class "o.a.c.m.util.Precision").
|
||||
|
|
|
|||
|
|
@ -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 <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
|
||||
* @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();
|
||||
}
|
||||
}
|
||||
|
|
@ -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"),
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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]"));
|
||||
}
|
||||
}
|
||||
|
|
@ -36,7 +36,7 @@ public class LocalizedFormatsTest {
|
|||
|
||||
@Test
|
||||
public void testMessageNumber() {
|
||||
Assert.assertEquals(310, LocalizedFormats.values().length);
|
||||
Assert.assertEquals(311, LocalizedFormats.values().length);
|
||||
}
|
||||
|
||||
@Test
|
||||
|
|
|
|||
Loading…
Reference in New Issue