You are viewing a plain text version of this content. The canonical link for it is here.
Posted to commits@commons.apache.org by er...@apache.org on 2018/12/15 03:42:20 UTC
[commons-geometry] 02/05: GEOMETRY-14: updating QuaternionRotation
to use Quaternion from commons-numbers
This is an automated email from the ASF dual-hosted git repository.
erans pushed a commit to branch feature_GEOMETRY-14
in repository https://gitbox.apache.org/repos/asf/commons-geometry.git
commit 97d8fb03e860ffd8103aaa4954e4cfd8be17241a
Author: Matt Juntunen <ma...@hotmail.com>
AuthorDate: Thu Dec 13 21:43:36 2018 -0500
GEOMETRY-14: updating QuaternionRotation to use Quaternion from commons-numbers
---
commons-geometry-euclidean/pom.xml | 4 +
.../threed/rotation/QuaternionRotation.java | 225 +++++++++------------
.../threed/rotation/QuaterionRotationTest.java | 144 +++++++++----
pom.xml | 5 +
4 files changed, 215 insertions(+), 163 deletions(-)
diff --git a/commons-geometry-euclidean/pom.xml b/commons-geometry-euclidean/pom.xml
index 4657729..5544db0 100644
--- a/commons-geometry-euclidean/pom.xml
+++ b/commons-geometry-euclidean/pom.xml
@@ -62,6 +62,10 @@
<groupId>org.apache.commons</groupId>
<artifactId>commons-numbers-angle</artifactId>
</dependency>
+ <dependency>
+ <groupId>org.apache.commons</groupId>
+ <artifactId>commons-numbers-quaternion</artifactId>
+ </dependency>
<dependency>
<groupId>org.apache.commons</groupId>
diff --git a/commons-geometry-euclidean/src/main/java/org/apache/commons/geometry/euclidean/threed/rotation/QuaternionRotation.java b/commons-geometry-euclidean/src/main/java/org/apache/commons/geometry/euclidean/threed/rotation/QuaternionRotation.java
index 0297779..b19c53c 100644
--- a/commons-geometry-euclidean/src/main/java/org/apache/commons/geometry/euclidean/threed/rotation/QuaternionRotation.java
+++ b/commons-geometry-euclidean/src/main/java/org/apache/commons/geometry/euclidean/threed/rotation/QuaternionRotation.java
@@ -17,6 +17,7 @@
package org.apache.commons.geometry.euclidean.threed.rotation;
import java.io.Serializable;
+import java.util.Objects;
import org.apache.commons.geometry.core.Geometry;
import org.apache.commons.geometry.core.exception.IllegalNormException;
@@ -26,12 +27,14 @@ import org.apache.commons.geometry.euclidean.internal.Vectors;
import org.apache.commons.geometry.euclidean.threed.AffineTransformMatrix3D;
import org.apache.commons.geometry.euclidean.threed.Vector3D;
import org.apache.commons.numbers.arrays.LinearCombination;
-import org.apache.commons.numbers.core.Precision;
+import org.apache.commons.numbers.quaternion.Quaternion;
/**
- * Class representing a unit-length quaternion used to model rotations in 3 dimensional
- * Euclidean space.
+ * Class using a unit-length quaternion to model rotations in 3 dimensional Euclidean space.
+ * The underlying quaternion is in <em>positive polar form</em>, meaing that it is normalized
+ * and has a non-negative scalar (ie, {@code w}) component .
*
+ * @see Quaternion
* @see <a href="https://en.wikipedia.org/wiki/Quaternions_and_spatial_rotation">Quaternion Rotations</a>
*/
public final class QuaternionRotation implements Serializable {
@@ -60,43 +63,27 @@ public final class QuaternionRotation implements Serializable {
/** Instance used to represent the identity rotation, ie a rotation with
* an angle of zero.
*/
- private static final QuaternionRotation IDENTITY_INSTANCE = new QuaternionRotation(0, 0, 0, 1);
+ private static final QuaternionRotation IDENTITY_INSTANCE = new QuaternionRotation(Quaternion.IDENTITY);
- /** First coordinate of the vectorial part of the quaternion. */
- private final double qx;
+ /** Unit-length quaternion instance in positive polar form. */
+ private final Quaternion quat;
- /** Second coordinate of the vectorial part of the quaternion. */
- private final double qy;
-
- /** Third coordinate of the vectorial part of the quaternion. */
- private final double qz;
-
- /** Scalar coordinate of the quaternion. */
- private final double qw;
+ /** Simple constructor. The given quaternion is converted to positive polar form.
+ * @param quat quaternion instance
+ * @throws IllegalStateException if the the norm of the given components is zero,
+ * NaN, or infinite
+ */
+ private QuaternionRotation(final Quaternion quat) {
+ this.quat = quat.positivePolarForm();
+ }
/**
- * Simple constructor. If the quaternion represents a rotation of greater than
- * {@code pi}, the values are inverted to keep the angle in the range
- * {@code [0, pi]}.
+ * Get the scalar coordinate of the underlying quaternion.
*
- * @param x first vectorial coordinate
- * @param y second vectorial coordinate
- * @param z third vectorial coordinate
- * @param w scalar coordinate
- */
- private QuaternionRotation(final double x, final double y, final double z, final double w) {
- if (w < 0.0) {
- this.qx = -x;
- this.qy = -y;
- this.qz = -z;
- this.qw = -w;
- }
- else {
- this.qx = x;
- this.qy = y;
- this.qz = z;
- this.qw = w;
- }
+ * @return scalar coordinate of the quaternion
+ */
+ public double getW() {
+ return quat.getW();
}
/**
@@ -105,7 +92,7 @@ public final class QuaternionRotation implements Serializable {
* @return first vectorial coordinate of the quaternion
*/
public double getX() {
- return qx;
+ return quat.getX();
}
/**
@@ -114,35 +101,33 @@ public final class QuaternionRotation implements Serializable {
* @return second vectorial coordinate of the quaternion
*/
public double getY() {
- return qy;
+ return quat.getY();
}
/**
- * Get the third vectorial coordinate of the quaternion.
+ * Get the third vectorial coordinate of the underlying quaternion.
*
* @return third vectorial coordinate of the quaternion
*/
public double getZ() {
- return qz;
+ return quat.getZ();
}
- /**
- * Get the scalar coordinate of the quaternion.
- *
- * @return scalar coordinate of the quaternion
+ /** Get the underlying quaternion instance.
+ * @return the quaternion instance
*/
- public double getW() {
- return qw;
+ public Quaternion getQuaternion() {
+ return quat;
}
/**
* Get the quaternion coordinates as an array in the order
- * {@code [x, y, z, w]}.
+ * {@code [w, x, y, z]}.
*
- * @return the quaternion coordinates in the order {@code [x, y, z, w]}
+ * @return the quaternion coordinates in the order {@code [w, x, y, z]}
*/
public double[] toArray() {
- return new double[] { qx, qy, qz, qw };
+ return new double[] { getW(), getX(), getY(), getZ() };
}
/**
@@ -157,7 +142,7 @@ public final class QuaternionRotation implements Serializable {
// the most straightforward way to check if we have a normalizable
// vector is to just try to normalize it and see if we fail
try {
- return Vector3D.normalize(qx, qy, qz);
+ return Vector3D.normalize(getX(), getY(), getZ());
}
catch (IllegalNormException exc) {
return Vector3D.PLUS_X;
@@ -171,7 +156,7 @@ public final class QuaternionRotation implements Serializable {
* @return The rotation angle in the range {@code [0, pi]}.
*/
public double getAngle() {
- return 2 * Math.acos(qw);
+ return 2 * Math.acos(getW());
}
/**
@@ -183,7 +168,7 @@ public final class QuaternionRotation implements Serializable {
* @return the negation (inverse) of the rotation
*/
public QuaternionRotation getInverse() {
- return new QuaternionRotation(-qx, -qy, -qz, qw);
+ return new QuaternionRotation(quat.conjugate());
}
/**
@@ -193,15 +178,20 @@ public final class QuaternionRotation implements Serializable {
* @return the rotated vector
*/
public Vector3D apply(final Vector3D v) {
+ final double qw = getW();
+ final double qx = getX();
+ final double qy = getY();
+ final double qz = getZ();
+
final double x = v.getX();
final double y = v.getY();
final double z = v.getZ();
// calculate the Hamilton product of the quaternion and vector
+ final double iw = -(qx * x) - (qy * y) - (qz * z);
final double ix = (qw * x) + (qy * z) - (qz * y);
final double iy = (qw * y) + (qz * x) - (qx * z);
final double iz = (qw * z) + (qx * y) - (qy * x);
- final double iw = -(qx * x) - (qy * y) - (qz * z);
// calculate the Hamilton product of the intermediate vector and
// the inverse quaternion
@@ -232,18 +222,8 @@ public final class QuaternionRotation implements Serializable {
* @return the result of multiplying this quaternion by the argument
*/
public QuaternionRotation multiply(final QuaternionRotation q) {
- // see
- // http://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions/code/index.htm#mul
-
- final QuaternionRotation a = this;
- final QuaternionRotation b = q;
-
- final double x = (a.qx * b.qw) + (a.qy * b.qz) - (a.qz * b.qy) + (a.qw * b.qx);
- final double y = -(a.qx * b.qz) + (a.qy * b.qw) + (a.qz * b.qx) + (a.qw * b.qy);
- final double z = (a.qx * b.qy) - (a.qy * b.qx) + (a.qz * b.qw) + (a.qw * b.qz);
- final double w = -(a.qx * b.qx) - (a.qy * b.qy) - (a.qz * b.qz) + (a.qw * b.qw);
-
- return new QuaternionRotation(x, y, z, w);
+ final Quaternion product = quat.multiply(q.quat);
+ return new QuaternionRotation(product);
}
/** Multiply the argument by this instance, returning the result as
@@ -298,12 +278,12 @@ public final class QuaternionRotation implements Serializable {
return end;
}
- double endX = end.qx;
- double endY = end.qy;
- double endZ = end.qz;
- double endW = end.qw;
+ double endW = end.getW();
+ double endX = end.getX();
+ double endY = end.getY();
+ double endZ = end.getZ();
- double dot = dotProduct(end);
+ double dot = quat.dotProduct(end.quat);
// If the dot product is negative, then the interpolation won't follow the shortest
// angular path between the two quaterions. In this case, invert the end quaternion
@@ -311,10 +291,10 @@ public final class QuaternionRotation implements Serializable {
if (dot < 0.0) {
dot = -dot;
+ endW = -endW;
endX = -endX;
endY = -endY;
endZ = -endZ;
- endW = -endW;
}
// If the quaternions are too closely aligned, perform linear interpolation
@@ -324,10 +304,10 @@ public final class QuaternionRotation implements Serializable {
final double oneMinusFactor = 1.0 - t;
return QuaternionRotation.of(
- (oneMinusFactor * this.qx) + (t * endX),
- (oneMinusFactor * this.qy) + (t * endY),
- (oneMinusFactor * this.qz) + (t * endZ),
- (oneMinusFactor * this.qw) + (t * endW));
+ (oneMinusFactor * this.getW()) + (t * endW),
+ (oneMinusFactor * this.getX()) + (t * endX),
+ (oneMinusFactor * this.getY()) + (t * endY),
+ (oneMinusFactor * this.getZ()) + (t * endZ));
}
// compute the standard slerp formula
@@ -337,12 +317,11 @@ public final class QuaternionRotation implements Serializable {
final double startFactor = Math.sin((1.0 - t) * theta) / sinTheta;
final double endFactor = Math.sin(t * theta) / sinTheta;
- return new QuaternionRotation(
- (startFactor * this.qx) + (endFactor * endX),
- (startFactor * this.qy) + (endFactor * endY),
- (startFactor * this.qz) + (endFactor * endZ),
- (startFactor * this.qw) + (endFactor * endW)
- );
+ return of(
+ (startFactor * this.getW()) + (endFactor * endW),
+ (startFactor * this.getX()) + (endFactor * endX),
+ (startFactor * this.getY()) + (endFactor * endY),
+ (startFactor * this.getZ()) + (endFactor * endZ));
}
/**
@@ -353,6 +332,12 @@ public final class QuaternionRotation implements Serializable {
* represented by this instance
*/
public AffineTransformMatrix3D toTransformMatrix() {
+
+ final double qw = getW();
+ final double qx = getX();
+ final double qy = getY();
+ final double qz = getZ();
+
// pre-calculate products that we'll need
final double xx = qx * qx;
final double xy = qx * qy;
@@ -438,10 +423,7 @@ public final class QuaternionRotation implements Serializable {
/** {@inheritDoc} */
@Override
public int hashCode() {
- return 131 * (193 * Double.hashCode(qx)) +
- (3 * Double.hashCode(qy)) +
- (5 * Double.hashCode(qz)) +
- (19 * Double.hashCode(qw));
+ return quat.hashCode();
}
/** {@inheritDoc} */
@@ -455,27 +437,13 @@ public final class QuaternionRotation implements Serializable {
}
QuaternionRotation other = (QuaternionRotation) obj;
- return Precision.equals(this.qx, other.qx) &&
- Precision.equals(this.qy, other.qy) &&
- Precision.equals(this.qz, other.qz) &&
- Precision.equals(this.qw, other.qw);
+ return Objects.equals(this.quat, other.quat);
}
/** {@inheritDoc} */
@Override
public String toString() {
- return SimpleTupleFormat.getDefault().format(qx, qy, qz, qw);
- }
-
- /** Compute the dot product of the current instance and the argument.
- * @param other instance to compute the dot product with
- * @return the dot product
- */
- private double dotProduct(final QuaternionRotation other) {
- return (this.qx * other.qx) +
- (this.qy * other.qy) +
- (this.qz * other.qz) +
- (this.qw * other.qw);
+ return SimpleTupleFormat.getDefault().format(getW(), getX(), getY(), getZ());
}
/** Get a sequence of angles around the given axes that produce a rotation equivalent
@@ -643,38 +611,45 @@ public final class QuaternionRotation implements Serializable {
return reverseArray(getRelativeEulerAngles(axis3, axis2, axis1));
}
+ /** Create a new instance from the given quaternion. The quaternion is normalized and
+ * converted to positive polar form (ie, with w >= 0).
+ *
+ * @param quat the quaternion to use for the rotation
+ * @return a new instance built from the given quaternion.
+ * @throws IllegalStateException if the the norm of the given components is zero,
+ * NaN, or infinite
+ * @see Quaternion#normalize()
+ * @see Quaternion#positivePolarForm()
+ */
+ public static QuaternionRotation of(Quaternion quat) {
+ return new QuaternionRotation(quat);
+ }
+
/**
* Create a new instance from the given quaternion values. The inputs are
- * normalized.
+ * normalized and converted to positive polar form (ie, with w >= 0).
*
+ * @param w quaterion scalar component
* @param x first quaternion vectorial component
* @param y second quaternion vectorial component
* @param z third quaternion vectorial component
- * @param w quaterion scalar component
* @return a new instance containing the normalized quaterion components
- * @throws IllegalNormException if the the norm of the given components is zero,
+ * @throws IllegalStateException if the the norm of the given components is zero,
* NaN, or infinite
+ * @see Quaternion#normalize()
+ * @see Quaternion#positivePolarForm()
*/
- public static QuaternionRotation of(final double x, final double y, final double z, final double w) {
- // normalize the inputs
- final double norm = Vectors.checkedNorm(Vectors.norm(x, y, z, w));
- final double invNorm = 1.0 / norm;
-
- final double qx = invNorm * x;
- final double qy = invNorm * y;
- final double qz = invNorm * z;
- final double qw = invNorm * w;
-
- return new QuaternionRotation(qx, qy, qz, qw);
+ public static QuaternionRotation of(final double w, final double x, final double y, final double z) {
+ return of(Quaternion.of(w, x, y, z));
}
/** Create a new instance from the components in the given 4-element array. The
- * components must be in the order {@code [x, y, z, w]}. The components are
+ * components must be in the order {@code [w, x, y, z]}. The components are
* normalized.
*
* @param q quaternion component array
* @return new quaterion containing the normalized components
- * @exception IllegalArgumentException if the array does not have 4 elements
+ * @throws IllegalArgumentException if the array does not have 4 elements
*/
public static QuaternionRotation of(final double[] q) {
if (q.length != 4) {
@@ -726,12 +701,12 @@ public final class QuaternionRotation implements Serializable {
final double halfAngle = 0.5 * angle;
final double sinHalfAngle = Math.sin(halfAngle);
+ final double w = Math.cos(halfAngle);
final double x = sinHalfAngle * normAxis.getX();
final double y = sinHalfAngle * normAxis.getY();
final double z = sinHalfAngle * normAxis.getZ();
- final double w = Math.cos(halfAngle);
- return new QuaternionRotation(x, y, z, w);
+ return of(w, x, y, z);
}
/** Return an instance that rotates the first vector to the second.
@@ -759,11 +734,11 @@ public final class QuaternionRotation implements Serializable {
// an arbitrary unit vector orthogonal to u1
final Vector3D axis = u.orthogonal();
- return new QuaternionRotation(
+ return of(
+ 0.0,
axis.getX(),
axis.getY(),
- axis.getZ(),
- 0.0
+ axis.getZ()
);
}
@@ -790,11 +765,11 @@ public final class QuaternionRotation implements Serializable {
final double vectorialScaleFactor = 1.0 / (2.0 * w * normProduct);
final Vector3D axis = u.crossProduct(v);
- return new QuaternionRotation(
+ return of(
+ w,
vectorialScaleFactor * axis.getX(),
vectorialScaleFactor * axis.getY(),
- vectorialScaleFactor * axis.getZ(),
- w
+ vectorialScaleFactor * axis.getZ()
);
}
@@ -954,7 +929,7 @@ public final class QuaternionRotation implements Serializable {
w = (m10 - m01) * sinv;
}
- return new QuaternionRotation(x, y, z, w);
+ return of(w, x, y, z);
}
/** Reverse the elements in {@code arr}. The array is returned.
diff --git a/commons-geometry-euclidean/src/test/java/org/apache/commons/geometry/euclidean/threed/rotation/QuaterionRotationTest.java b/commons-geometry-euclidean/src/test/java/org/apache/commons/geometry/euclidean/threed/rotation/QuaterionRotationTest.java
index c0e405b..a87d351 100644
--- a/commons-geometry-euclidean/src/test/java/org/apache/commons/geometry/euclidean/threed/rotation/QuaterionRotationTest.java
+++ b/commons-geometry-euclidean/src/test/java/org/apache/commons/geometry/euclidean/threed/rotation/QuaterionRotationTest.java
@@ -35,6 +35,9 @@ import org.apache.commons.geometry.euclidean.threed.rotation.AxisSequence;
import org.apache.commons.geometry.euclidean.threed.rotation.QuaternionRotation;
import org.apache.commons.numbers.angle.PlaneAngleRadians;
import org.apache.commons.numbers.core.Precision;
+import org.apache.commons.numbers.quaternion.Quaternion;
+import org.apache.commons.rng.UniformRandomProvider;
+import org.apache.commons.rng.simple.RandomSource;
import org.junit.Assert;
import org.junit.Test;
@@ -59,54 +62,86 @@ public class QuaterionRotationTest {
private static final double MINUS_TWO_THIRDS_PI = -TWO_THIRDS_PI;
@Test
- public void testOf() {
+ public void testOf_quaternion() {
+ // act/assert
+ checkQuaternion(QuaternionRotation.of(Quaternion.of(1, 0, 0, 0)), 1, 0, 0, 0);
+ checkQuaternion(QuaternionRotation.of(Quaternion.of(-1, 0, 0, 0)), 1, 0, 0, 0);
+ checkQuaternion(QuaternionRotation.of(Quaternion.of(0, 1, 0, 0)), 0, 1, 0, 0);
+ checkQuaternion(QuaternionRotation.of(Quaternion.of(0, 0, 1, 0)), 0, 0, 1, 0);
+ checkQuaternion(QuaternionRotation.of(Quaternion.of(0, 0, 0, 1)), 0, 0, 0, 1);
+
+ checkQuaternion(QuaternionRotation.of(Quaternion.of(1, 1, 1, 1)), 0.5, 0.5, 0.5, 0.5);
+ checkQuaternion(QuaternionRotation.of(Quaternion.of(-1, -1, -1, -1)), 0.5, 0.5, 0.5, 0.5);
+ }
+
+ @Test
+ public void testOf_quaternion_illegalNorm() {
+ // act/assert
+ GeometryTestUtils.assertThrows(() ->
+ QuaternionRotation.of(Quaternion.of(0, 0, 0, 0)), IllegalStateException.class);
+ GeometryTestUtils.assertThrows(() ->
+ QuaternionRotation.of(Quaternion.of(1, 1, 1, Double.NaN)), IllegalStateException.class);
+ GeometryTestUtils.assertThrows(() ->
+ QuaternionRotation.of(Quaternion.of(1, 1, Double.POSITIVE_INFINITY, 1)), IllegalStateException.class);
+ GeometryTestUtils.assertThrows(() ->
+ QuaternionRotation.of(Quaternion.of(1, Double.NEGATIVE_INFINITY, 1, 1)), IllegalStateException.class);
+ GeometryTestUtils.assertThrows(() ->
+ QuaternionRotation.of(Quaternion.of(Double.NaN, 1, 1, 1)), IllegalStateException.class);
+ }
+
+ @Test
+ public void testOf_components() {
// act/assert
checkQuaternion(QuaternionRotation.of(1, 0, 0, 0), 1, 0, 0, 0);
+ checkQuaternion(QuaternionRotation.of(-1, 0, 0, 0), 1, 0, 0, 0);
checkQuaternion(QuaternionRotation.of(0, 1, 0, 0), 0, 1, 0, 0);
checkQuaternion(QuaternionRotation.of(0, 0, 1, 0), 0, 0, 1, 0);
checkQuaternion(QuaternionRotation.of(0, 0, 0, 1), 0, 0, 0, 1);
checkQuaternion(QuaternionRotation.of(1, 1, 1, 1), 0.5, 0.5, 0.5, 0.5);
+ checkQuaternion(QuaternionRotation.of(-1, -1, -1, -1), 0.5, 0.5, 0.5, 0.5);
}
@Test
- public void testOf_illegalNorm() {
+ public void testOf_components_illegalNorm() {
// act/assert
- GeometryTestUtils.assertThrows(() -> QuaternionRotation.of(0, 0, 0, 0), IllegalNormException.class);
- GeometryTestUtils.assertThrows(() -> QuaternionRotation.of(1, 1, 1, Double.NaN), IllegalNormException.class);
- GeometryTestUtils.assertThrows(() -> QuaternionRotation.of(1, 1, Double.POSITIVE_INFINITY, 1), IllegalNormException.class);
- GeometryTestUtils.assertThrows(() -> QuaternionRotation.of(1, Double.NEGATIVE_INFINITY, 1, 1), IllegalNormException.class);
- GeometryTestUtils.assertThrows(() -> QuaternionRotation.of(Double.NaN, 1, 1, 1), IllegalNormException.class);
+ GeometryTestUtils.assertThrows(() -> QuaternionRotation.of(0, 0, 0, 0), IllegalStateException.class);
+ GeometryTestUtils.assertThrows(() -> QuaternionRotation.of(1, 1, 1, Double.NaN), IllegalStateException.class);
+ GeometryTestUtils.assertThrows(() -> QuaternionRotation.of(1, 1, Double.POSITIVE_INFINITY, 1), IllegalStateException.class);
+ GeometryTestUtils.assertThrows(() -> QuaternionRotation.of(1, Double.NEGATIVE_INFINITY, 1, 1), IllegalStateException.class);
+ GeometryTestUtils.assertThrows(() -> QuaternionRotation.of(Double.NaN, 1, 1, 1), IllegalStateException.class);
}
@Test
- public void testOf_arrayArg() {
+ public void testOf_array() {
// act/assert
checkQuaternion(QuaternionRotation.of(new double[] { 1, 0, 0, 0 }), 1, 0, 0, 0);
+ checkQuaternion(QuaternionRotation.of(new double[] { -1, 0, 0, 0 }), 1, 0, 0, 0);
checkQuaternion(QuaternionRotation.of(new double[] { 0, 1, 0, 0 }), 0, 1, 0, 0);
checkQuaternion(QuaternionRotation.of(new double[] { 0, 0, 1, 0 }), 0, 0, 1, 0);
checkQuaternion(QuaternionRotation.of(new double[] { 0, 0, 0, 1 }), 0, 0, 0, 1);
checkQuaternion(QuaternionRotation.of(new double[] { 1, 1, 1, 1 }), 0.5, 0.5, 0.5, 0.5);
+ checkQuaternion(QuaternionRotation.of(new double[] { -1, -1, -1, -1 }), 0.5, 0.5, 0.5, 0.5);
}
@Test
- public void testOf_arrayArg_illegalNorm() {
+ public void testOf_array_illegalNorm() {
// act/assert
GeometryTestUtils.assertThrows(() -> QuaternionRotation.of(new double[] { 0, 0, 0, 0 }),
- IllegalNormException.class);
+ IllegalStateException.class);
GeometryTestUtils.assertThrows(() -> QuaternionRotation.of(new double[] { 1, 1, 1, Double.NaN }),
- IllegalNormException.class);
+ IllegalStateException.class);
GeometryTestUtils.assertThrows(() -> QuaternionRotation.of(new double[] { 1, 1, Double.POSITIVE_INFINITY, 1 }),
- IllegalNormException.class);
+ IllegalStateException.class);
GeometryTestUtils.assertThrows(() -> QuaternionRotation.of(new double[] { 1, Double.NEGATIVE_INFINITY, 1, 1 }),
- IllegalNormException.class);
+ IllegalStateException.class);
GeometryTestUtils.assertThrows(() -> QuaternionRotation.of(new double[] { Double.NaN, 1, 1, 1 }),
- IllegalNormException.class);
+ IllegalStateException.class);
}
@Test
- public void testOf_arrayArg_invalidLength() {
+ public void testOf_array_invalidLength() {
// act/assert
GeometryTestUtils.assertThrows(() -> QuaternionRotation.of(new double[] { 1, 1, 1 }),
IllegalArgumentException.class, "Dimension mismatch: 3 != 4");
@@ -151,20 +186,20 @@ public class QuaterionRotationTest {
public void testGetAxis()
{
// act/assert
- checkVector(QuaternionRotation.of(1, 0, 0, 0).getAxis(), 1, 0, 0);
- checkVector(QuaternionRotation.of(-1, 0, 0, 0).getAxis(), -1, 0, 0);
+ checkVector(QuaternionRotation.of(0, 1, 0, 0).getAxis(), 1, 0, 0);
+ checkVector(QuaternionRotation.of(0, -1, 0, 0).getAxis(), -1, 0, 0);
- checkVector(QuaternionRotation.of(0, 1, 0, 0).getAxis(), 0, 1, 0);
- checkVector(QuaternionRotation.of(0, -1, 0, 0).getAxis(), 0, -1, 0);
+ checkVector(QuaternionRotation.of(0, 0, 1, 0).getAxis(), 0, 1, 0);
+ checkVector(QuaternionRotation.of(0, 0, -1, 0).getAxis(), 0, -1, 0);
- checkVector(QuaternionRotation.of(0, 0, 1, 0).getAxis(), 0, 0, 1);
- checkVector(QuaternionRotation.of(0, 0, -1, 0).getAxis(), 0, 0, -1);
+ checkVector(QuaternionRotation.of(0, 0, 0, 1).getAxis(), 0, 0, 1);
+ checkVector(QuaternionRotation.of(0, 0, 0, -1).getAxis(), 0, 0, -1);
}
@Test
public void testGetAxis_noAxis() {
// arrange
- QuaternionRotation rot = QuaternionRotation.of(0, 0, 0, 1);
+ QuaternionRotation rot = QuaternionRotation.of(1, 0, 0, 0);
// act/assert
EuclideanTestUtils.assertCoordinatesEqual(Vector3D.PLUS_X, rot.getAxis(), EPS);
@@ -192,15 +227,15 @@ public class QuaterionRotationTest {
@Test
public void testGetAngle() {
// act/assert
- Assert.assertEquals(Geometry.ZERO_PI, QuaternionRotation.of(0, 0, 0, 1).getAngle(), EPS);
- Assert.assertEquals(Geometry.ZERO_PI, QuaternionRotation.of(0, 0, 0, -1).getAngle(), EPS);
+ Assert.assertEquals(Geometry.ZERO_PI, QuaternionRotation.of(1, 0, 0, 0).getAngle(), EPS);
+ Assert.assertEquals(Geometry.ZERO_PI, QuaternionRotation.of(-1, 0, 0, 0).getAngle(), EPS);
Assert.assertEquals(Geometry.HALF_PI, QuaternionRotation.of(1, 0, 0, 1).getAngle(), EPS);
Assert.assertEquals(Geometry.HALF_PI, QuaternionRotation.of(-1, 0, 0, -1).getAngle(), EPS);
Assert.assertEquals(Geometry.PI * 2.0 / 3.0, QuaternionRotation.of(1, 1, 1, 1).getAngle(), EPS);
- Assert.assertEquals(Geometry.PI, QuaternionRotation.of(1, 0, 0, 0).getAngle(), EPS);
+ Assert.assertEquals(Geometry.PI, QuaternionRotation.of(0, 0, 0, 1).getAngle(), EPS);
}
@Test
@@ -449,6 +484,32 @@ public class QuaterionRotationTest {
}
@Test
+ public void testMultiply_numericalStability() {
+ // arrange
+ int slices = 1024;
+ double delta = (8.0 * Geometry.PI / 3.0) / slices;
+
+ QuaternionRotation q = QuaternionRotation.identity();
+
+ UniformRandomProvider rand = RandomSource.create(RandomSource.JDK, 2L);
+
+ // act
+ for (int i=0; i<slices; ++i) {
+ double angle = rand.nextDouble();
+ QuaternionRotation forward = QuaternionRotation.fromAxisAngle(PLUS_DIAGONAL, angle);
+ QuaternionRotation backward = QuaternionRotation.fromAxisAngle(PLUS_DIAGONAL, delta - angle);
+
+ q = q.multiply(forward).multiply(backward);
+ }
+
+ // assert
+ Assert.assertTrue(q.getW() > 0);
+ Assert.assertTrue(q.getQuaternion().isUnitQuaternion(EPS));
+
+ assertRotationEquals(StandardRotations.PLUS_DIAGONAL_TWO_THIRDS_PI, q);
+ }
+
+ @Test
public void testPremultiply_sameAxis_simple() {
// arrange
QuaternionRotation q1 = QuaternionRotation.fromAxisAngle(Vector3D.PLUS_X, 0.1 * Geometry.PI);
@@ -627,8 +688,8 @@ public class QuaterionRotationTest {
@Test
public void testSlerp_quaternionsHaveMinusOneDotProduct() {
// arrange
- QuaternionRotation q1 = QuaternionRotation.of(0, 0, 1, 1); // pi/2 around +z
- QuaternionRotation q2 = QuaternionRotation.of(0, 0, -1, -1); // 3pi/2 around -z
+ QuaternionRotation q1 = QuaternionRotation.of(1, 0, 0, 1); // pi/2 around +z
+ QuaternionRotation q2 = QuaternionRotation.of(-1, 0, 0, -1); // 3pi/2 around -z
// act
QuaternionRotation result = q1.slerp(q2, 0.5);
@@ -780,7 +841,7 @@ public class QuaterionRotationTest {
QuaternionRotation result = QuaternionRotation.fromAxisAngleSequence(seq);
// assert
- checkQuaternion(result, q.getX(), q.getY(), q.getZ(), q.getW());
+ checkQuaternion(result, q.getW(), q.getX(), q.getY(), q.getZ());
}
});
}
@@ -817,7 +878,7 @@ public class QuaterionRotationTest {
assertRadiansEquals(singularityAngle, resultSeq.getAngle2());
assertRadiansEquals(0.0, resultSeq.getAngle3());
- checkQuaternion(resultQuat, inputQuat.getX(), inputQuat.getY(), inputQuat.getZ(), inputQuat.getW());
+ checkQuaternion(resultQuat, inputQuat.getW(), inputQuat.getX(), inputQuat.getY(), inputQuat.getZ());
}
}
}
@@ -854,7 +915,7 @@ public class QuaterionRotationTest {
assertRadiansEquals(0.0, resultSeq.getAngle1());
assertRadiansEquals(singularityAngle, resultSeq.getAngle2());
- checkQuaternion(resultQuat, inputQuat.getX(), inputQuat.getY(), inputQuat.getZ(), inputQuat.getW());
+ checkQuaternion(resultQuat, inputQuat.getW(), inputQuat.getX(), inputQuat.getY(), inputQuat.getZ());
}
}
}
@@ -891,7 +952,7 @@ public class QuaterionRotationTest {
assertRadiansEquals(singularityAngle, resultSeq.getAngle2());
assertRadiansEquals(0.0, resultSeq.getAngle3());
- checkQuaternion(resultQuat, inputQuat.getX(), inputQuat.getY(), inputQuat.getZ(), inputQuat.getW());
+ checkQuaternion(resultQuat, inputQuat.getW(), inputQuat.getX(), inputQuat.getY(), inputQuat.getZ());
}
}
}
@@ -928,7 +989,7 @@ public class QuaterionRotationTest {
assertRadiansEquals(0.0, resultSeq.getAngle1());
assertRadiansEquals(singularityAngle, resultSeq.getAngle2());
- checkQuaternion(resultQuat, inputQuat.getX(), inputQuat.getY(), inputQuat.getZ(), inputQuat.getW());
+ checkQuaternion(resultQuat, inputQuat.getW(), inputQuat.getX(), inputQuat.getY(), inputQuat.getZ());
}
}
}
@@ -1046,7 +1107,7 @@ public class QuaterionRotationTest {
// assert
double val = Math.sqrt(2) * 0.5;
- checkQuaternion(q, 0, 0, val, val);
+ checkQuaternion(q, val, 0, 0, val);
EuclideanTestUtils.assertCoordinatesEqual(Vector3D.PLUS_Z, q.getAxis(), EPS);
Assert.assertEquals(Geometry.HALF_PI, q.getAngle(), EPS);
@@ -1065,7 +1126,7 @@ public class QuaterionRotationTest {
QuaternionRotation q = QuaternionRotation.createVectorRotation(u1, u2);
// assert
- checkQuaternion(q, 0, 0, 0, 1);
+ checkQuaternion(q, 1, 0, 0, 0);
EuclideanTestUtils.assertCoordinatesEqual(Vector3D.PLUS_X, q.getAxis(), EPS);
Assert.assertEquals(Geometry.ZERO_PI, q.getAngle(), EPS);
@@ -1084,7 +1145,7 @@ public class QuaterionRotationTest {
QuaternionRotation q = QuaternionRotation.createVectorRotation(u1, u2);
// assert
- checkQuaternion(q, 0, 0, 0, 1);
+ checkQuaternion(q, 1, 0, 0, 0);
EuclideanTestUtils.assertCoordinatesEqual(Vector3D.PLUS_X, q.getAxis(), EPS);
Assert.assertEquals(Geometry.ZERO_PI, q.getAngle(), EPS);
@@ -1545,13 +1606,20 @@ public class QuaterionRotationTest {
assertRotationEquals(rotation, QuaternionRotation.fromAxisAngleSequence(angles));
}
- private static void checkQuaternion(QuaternionRotation q, double x, double y, double z, double w) {
- String msg = "Expected quaternion to equal " + SimpleTupleFormat.getDefault().format(x, y, z, w) + " but was " + q;
+ private static void checkQuaternion(QuaternionRotation qrot, double w, double x, double y, double z) {
+ String msg = "Expected"
+ + " quaternion to equal " + SimpleTupleFormat.getDefault().format(w, x, y, z) + " but was " + qrot;
+
+ Assert.assertEquals(msg, w, qrot.getW(), EPS);
+ Assert.assertEquals(msg, x, qrot.getX(), EPS);
+ Assert.assertEquals(msg, y, qrot.getY(), EPS);
+ Assert.assertEquals(msg, z, qrot.getZ(), EPS);
+ Quaternion q = qrot.getQuaternion();
+ Assert.assertEquals(msg, w, q.getW(), EPS);
Assert.assertEquals(msg, x, q.getX(), EPS);
Assert.assertEquals(msg, y, q.getY(), EPS);
Assert.assertEquals(msg, z, q.getZ(), EPS);
- Assert.assertEquals(msg, w, q.getW(), EPS);
}
private static void checkVector(Vector3D v, double x, double y, double z) {
diff --git a/pom.xml b/pom.xml
index f93cd6f..d9ea303 100644
--- a/pom.xml
+++ b/pom.xml
@@ -163,6 +163,11 @@
<artifactId>commons-numbers-fraction</artifactId>
<version>${commons.numbers.version}</version>
</dependency>
+ <dependency>
+ <groupId>org.apache.commons</groupId>
+ <artifactId>commons-numbers-quaternion</artifactId>
+ <version>${commons.numbers.version}</version>
+ </dependency>
<dependency>
<groupId>org.apache.commons</groupId>