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:38:08 UTC
[commons-geometry] 01/04: GEOMETRY-14: Adding
AffineTransformMatrix?D classes, QuaternionRotation, and AxisAngleSequence;
removing previous Rotation class
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 c7f8a3d5c2db162e1ef0f3ff63aec590de298cc9
Author: Matt Juntunen <ma...@hotmail.com>
AuthorDate: Mon Dec 10 21:23:14 2018 -0500
GEOMETRY-14: Adding AffineTransformMatrix?D classes, QuaternionRotation, and AxisAngleSequence; removing previous Rotation class
---
.../org/apache/commons/geometry/core/Geometry.java | 5 +
.../geometry/core/internal/SimpleTupleFormat.java | 32 +
.../apache/commons/geometry/core/GeometryTest.java | 5 +
.../core/internal/SimpleTupleFormatTest.java | 22 +
.../exception/NonInvertibleTransformException.java | 35 +
.../geometry/euclidean/exception/package-info.java | 23 +
.../geometry/euclidean/internal/Matrices.java | 62 +
.../geometry/euclidean/internal/Vectors.java | 29 +
.../euclidean/oned/AffineTransformMatrix1D.java | 379 +++++
.../commons/geometry/euclidean/oned/Vector1D.java | 10 +
.../euclidean/threed/AffineTransformMatrix3D.java | 629 ++++++++
.../commons/geometry/euclidean/threed/Plane.java | 15 +-
.../geometry/euclidean/threed/PolyhedronsSet.java | 13 +-
.../geometry/euclidean/threed/Rotation.java | 1448 ------------------
.../euclidean/threed/RotationConvention.java | 78 -
.../geometry/euclidean/threed/RotationOrder.java | 172 ---
.../geometry/euclidean/threed/Vector3D.java | 10 +
.../threed/rotation/AxisAngleSequence.java | 210 +++
.../threed/rotation/AxisReferenceFrame.java | 55 +
.../euclidean/threed/rotation/AxisSequence.java | 146 ++
.../threed/rotation/AxisSequenceType.java | 38 +
.../threed/rotation/QuaternionRotation.java | 980 ++++++++++++
.../euclidean/threed/rotation/package-info.java | 47 +
.../euclidean/twod/AffineTransformMatrix2D.java | 552 +++++++
.../commons/geometry/euclidean/twod/Vector2D.java | 10 +
.../geometry/euclidean/EuclideanTestUtils.java | 207 ++-
.../geometry/euclidean/internal/MatricesTest.java | 88 ++
.../geometry/euclidean/internal/VectorsTest.java | 24 +
.../oned/AffineTransformMatrix1DTest.java | 649 ++++++++
.../geometry/euclidean/oned/IntervalTest.java | 1 -
.../geometry/euclidean/oned/Vector1DTest.java | 15 +
.../threed/AffineTransformMatrix3DTest.java | 957 ++++++++++++
.../geometry/euclidean/threed/PlaneTest.java | 11 +-
.../euclidean/threed/PolyhedronsSetTest.java | 13 +-
.../euclidean/threed/RotationOrderTest.java | 59 -
.../geometry/euclidean/threed/RotationTest.java | 815 ----------
.../geometry/euclidean/threed/Vector3DTest.java | 15 +
.../threed/rotation/AxisAngleSequenceTest.java | 124 ++
.../threed/rotation/AxisSequenceTest.java | 63 +
.../threed/rotation/QuaterionRotationTest.java | 1613 ++++++++++++++++++++
.../threed/rotation/StandardRotations.java | 66 +
.../twod/AffineTransformMatrix2DTest.java | 963 ++++++++++++
.../geometry/euclidean/twod/Vector2DTest.java | 15 +
.../commons/geometry/spherical/twod/Circle.java | 16 +-
.../spherical/twod/SphericalPolygonsSet.java | 13 +-
.../geometry/spherical/twod/CircleTest.java | 18 +-
.../spherical/twod/SphericalPolygonsSetTest.java | 4 +-
.../geometry/spherical/twod/SubCircleTest.java | 4 +-
48 files changed, 8088 insertions(+), 2670 deletions(-)
diff --git a/commons-geometry-core/src/main/java/org/apache/commons/geometry/core/Geometry.java b/commons-geometry-core/src/main/java/org/apache/commons/geometry/core/Geometry.java
index fe319c4..819a9e5 100644
--- a/commons-geometry-core/src/main/java/org/apache/commons/geometry/core/Geometry.java
+++ b/commons-geometry-core/src/main/java/org/apache/commons/geometry/core/Geometry.java
@@ -41,6 +41,11 @@ public final class Geometry {
/** Constant value for {@code 3*pi/2}. */
public static final double THREE_HALVES_PI = 1.5 * Math.PI;
+ /** Constant value for {@code 0*pi}, which is, of course, 0.
+ * This value is placed here for completeness.
+ */
+ public static final double ZERO_PI = 0.0;
+
/** Private constructor */
private Geometry() {}
}
diff --git a/commons-geometry-core/src/main/java/org/apache/commons/geometry/core/internal/SimpleTupleFormat.java b/commons-geometry-core/src/main/java/org/apache/commons/geometry/core/internal/SimpleTupleFormat.java
index b2ecfc5..15637e0 100644
--- a/commons-geometry-core/src/main/java/org/apache/commons/geometry/core/internal/SimpleTupleFormat.java
+++ b/commons-geometry-core/src/main/java/org/apache/commons/geometry/core/internal/SimpleTupleFormat.java
@@ -160,6 +160,38 @@ public class SimpleTupleFormat {
return sb.toString();
}
+ /** Return a tuple string with the given values.
+ * @param a1 first value
+ * @param a2 second value
+ * @param a3 third value
+ * @param a4 fourth value
+ * @return 4-tuple string
+ */
+ public String format(double a1, double a2, double a3, double a4) {
+ final StringBuilder sb = new StringBuilder();
+
+ if (prefix != null) {
+ sb.append(prefix);
+ }
+
+ sb.append(a1)
+ .append(separator)
+ .append(SPACE)
+ .append(a2)
+ .append(separator)
+ .append(SPACE)
+ .append(a3)
+ .append(separator)
+ .append(SPACE)
+ .append(a4);
+
+ if (suffix != null) {
+ sb.append(suffix);
+ }
+
+ return sb.toString();
+ }
+
/** Parse the given string as a 1-tuple and passes the tuple values to the
* given function. The function output is returned.
* @param <T> function return type
diff --git a/commons-geometry-core/src/test/java/org/apache/commons/geometry/core/GeometryTest.java b/commons-geometry-core/src/test/java/org/apache/commons/geometry/core/GeometryTest.java
index 61fc872..f1667ff 100644
--- a/commons-geometry-core/src/test/java/org/apache/commons/geometry/core/GeometryTest.java
+++ b/commons-geometry-core/src/test/java/org/apache/commons/geometry/core/GeometryTest.java
@@ -27,6 +27,8 @@ public class GeometryTest {
double eps = 0.0;
// act/assert
+ Assert.assertEquals(0.0, Geometry.ZERO_PI, eps);
+
Assert.assertEquals(Math.PI, Geometry.PI, eps);
Assert.assertEquals(-Math.PI, Geometry.MINUS_PI, eps);
@@ -45,6 +47,9 @@ public class GeometryTest {
double eps = 1e-15;
// act/assert
+ Assert.assertEquals(0.0, Math.sin(Geometry.ZERO_PI), eps);
+ Assert.assertEquals(1.0, Math.cos(Geometry.ZERO_PI), eps);
+
Assert.assertEquals(0.0, Math.sin(Geometry.PI), eps);
Assert.assertEquals(-1.0, Math.cos(Geometry.PI), eps);
diff --git a/commons-geometry-core/src/test/java/org/apache/commons/geometry/core/internal/SimpleTupleFormatTest.java b/commons-geometry-core/src/test/java/org/apache/commons/geometry/core/internal/SimpleTupleFormatTest.java
index 6f1467d..acbb022 100644
--- a/commons-geometry-core/src/test/java/org/apache/commons/geometry/core/internal/SimpleTupleFormatTest.java
+++ b/commons-geometry-core/src/test/java/org/apache/commons/geometry/core/internal/SimpleTupleFormatTest.java
@@ -157,6 +157,28 @@ public class SimpleTupleFormatTest {
}
@Test
+ public void testFormat4D() {
+ // arrange
+ SimpleTupleFormat formatter = new SimpleTupleFormat(OPEN_PAREN, CLOSE_PAREN);
+
+ // act/assert
+ Assert.assertEquals("(1.0, 0.0, -1.0, 2.0)", formatter.format(1.0, 0.0, -1.0, 2.0));
+ Assert.assertEquals("(-1.0, 1.0, 0.0, 2.0)", formatter.format(-1.0, 1.0, 0.0, 2.0));
+ Assert.assertEquals("(NaN, -Infinity, Infinity, NaN)", formatter.format(Double.NaN, Double.NEGATIVE_INFINITY, Double.POSITIVE_INFINITY, Double.NaN));
+ }
+
+ @Test
+ public void testFormat4D_noPrefixSuffix() {
+ // arrange
+ SimpleTupleFormat formatter = new SimpleTupleFormat(null, null);
+
+ // act/assert
+ Assert.assertEquals("1.0, 0.0, -1.0, 2.0", formatter.format(1.0, 0.0, -1.0, 2.0));
+ Assert.assertEquals("-1.0, 1.0, 0.0, 2.0", formatter.format(-1.0, 1.0, 0.0, 2.0));
+ Assert.assertEquals("NaN, -Infinity, Infinity, NaN", formatter.format(Double.NaN, Double.NEGATIVE_INFINITY, Double.POSITIVE_INFINITY, Double.NaN));
+ }
+
+ @Test
public void testFormat_longTokens() {
// arrange
SimpleTupleFormat formatter = new SimpleTupleFormat("||", "<<", ">>");
diff --git a/commons-geometry-euclidean/src/main/java/org/apache/commons/geometry/euclidean/exception/NonInvertibleTransformException.java b/commons-geometry-euclidean/src/main/java/org/apache/commons/geometry/euclidean/exception/NonInvertibleTransformException.java
new file mode 100644
index 0000000..88748dd
--- /dev/null
+++ b/commons-geometry-euclidean/src/main/java/org/apache/commons/geometry/euclidean/exception/NonInvertibleTransformException.java
@@ -0,0 +1,35 @@
+/*
+ * 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.geometry.euclidean.exception;
+
+import org.apache.commons.geometry.core.exception.GeometryException;
+
+/** Exception thrown when a transform matrix is not
+ * able to be inverted.
+ */
+public class NonInvertibleTransformException extends GeometryException {
+
+ /** Serializable version identifier */
+ private static final long serialVersionUID = 20180927L;
+
+ /** Simple constructor accepting an error message.
+ * @param msg error message
+ */
+ public NonInvertibleTransformException(String msg) {
+ super(msg);
+ }
+}
diff --git a/commons-geometry-euclidean/src/main/java/org/apache/commons/geometry/euclidean/exception/package-info.java b/commons-geometry-euclidean/src/main/java/org/apache/commons/geometry/euclidean/exception/package-info.java
new file mode 100644
index 0000000..d441d1d
--- /dev/null
+++ b/commons-geometry-euclidean/src/main/java/org/apache/commons/geometry/euclidean/exception/package-info.java
@@ -0,0 +1,23 @@
+/*
+ * 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.
+ */
+/**
+ *
+ * <p>
+ * This package provides exception types for Euclidean space.
+ * </p>
+ */
+package org.apache.commons.geometry.euclidean.exception;
diff --git a/commons-geometry-euclidean/src/main/java/org/apache/commons/geometry/euclidean/internal/Matrices.java b/commons-geometry-euclidean/src/main/java/org/apache/commons/geometry/euclidean/internal/Matrices.java
new file mode 100644
index 0000000..ec710ba
--- /dev/null
+++ b/commons-geometry-euclidean/src/main/java/org/apache/commons/geometry/euclidean/internal/Matrices.java
@@ -0,0 +1,62 @@
+/*
+ * 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.geometry.euclidean.internal;
+
+/** This class consists exclusively of static matrix utility methods.
+ */
+public final class Matrices {
+
+ /** Private constructor */
+ private Matrices() {}
+
+ /** Compute the determinant of the 2x2 matrix represented by the given values.
+ * The values are listed in row-major order.
+ * @param a00 matrix entry <code>a<sub>0,0</sub></code>
+ * @param a01 matrix entry <code>a<sub>0,1</sub></code>
+ * @param a10 matrix entry <code>a<sub>1,0</sub></code>
+ * @param a11 matrix entry <code>a<sub>1,1</sub></code>
+ * @return computed 2x2 matrix determinant
+ */
+ public static double determinant(
+ final double a00, final double a01,
+ final double a10, final double a11) {
+
+ return (a00 * a11) - (a01 * a10);
+ }
+
+ /** Compute the determinant of the 3x3 matrix represented by the given values.
+ * The values are listed in row-major order.
+ * @param a00 matrix entry <code>a<sub>0,0</sub></code>
+ * @param a01 matrix entry <code>a<sub>0,1</sub></code>
+ * @param a02 matrix entry <code>a<sub>0,2</sub></code>
+ * @param a10 matrix entry <code>a<sub>1,0</sub></code>
+ * @param a11 matrix entry <code>a<sub>1,1</sub></code>
+ * @param a12 matrix entry <code>a<sub>1,2</sub></code>
+ * @param a20 matrix entry <code>a<sub>2,0</sub></code>
+ * @param a21 matrix entry <code>a<sub>2,1</sub></code>
+ * @param a22 matrix entry <code>a<sub>2,2</sub></code>
+ * @return computed 3x3 matrix determinant
+ */
+ public static double determinant(
+ final double a00, final double a01, final double a02,
+ final double a10, final double a11, final double a12,
+ final double a20, final double a21, final double a22) {
+
+ return ((a00 * a11 * a22) + (a01 * a12 * a20) + (a02 * a10 * a21)) -
+ ((a00 * a12 * a21) + (a01 * a10 * a22) + (a02 * a11 * a20));
+ }
+}
diff --git a/commons-geometry-euclidean/src/main/java/org/apache/commons/geometry/euclidean/internal/Vectors.java b/commons-geometry-euclidean/src/main/java/org/apache/commons/geometry/euclidean/internal/Vectors.java
index ddf5883..91c5793 100644
--- a/commons-geometry-euclidean/src/main/java/org/apache/commons/geometry/euclidean/internal/Vectors.java
+++ b/commons-geometry-euclidean/src/main/java/org/apache/commons/geometry/euclidean/internal/Vectors.java
@@ -99,6 +99,21 @@ public final class Vectors {
return Math.sqrt(normSq(x1, x2, x3));
}
+ /** Get the L<sub>2</sub> norm (commonly known as the Euclidean norm) for the vector
+ * with the given components. This corresponds to the common notion of vector magnitude
+ * or length and is defined as the square root of the sum of the squares of all vector components.
+ * @param x1 first vector component
+ * @param x2 second vector component
+ * @param x3 third vector component
+ * @param x4 fourth vector component
+ * @return L<sub>2</sub> norm for the vector with the given components
+ * @see <a href="http://mathworld.wolfram.com/L2-Norm.html">L2 Norm</a>
+ */
+ public static double norm(final double x1, final double x2, final double x3, final double x4) {
+ return Math.sqrt(normSq(x1, x2, x3, x4));
+ }
+
+
/** Get the square of the L<sub>2</sub> norm (also known as the Euclidean norm)
* for the vector with the given components. This is equal to the sum of the squares of
* all vector components.
@@ -134,4 +149,18 @@ public final class Vectors {
public static double normSq(final double x1, final double x2, final double x3) {
return (x1 * x1) + (x2 * x2) + (x3 * x3);
}
+
+ /** Get the square of the L<sub>2</sub> norm (also known as the Euclidean norm)
+ * for the vector with the given components. This is equal to the sum of the squares of
+ * all vector components.
+ * @param x1 first vector component
+ * @param x2 second vector component
+ * @param x3 third vector component
+ * @param x4 fourth vector component
+ * @return square of the L<sub>2</sub> norm for the vector with the given components
+ * @see #norm(double, double, double, double)
+ */
+ public static double normSq(final double x1, final double x2, final double x3, final double x4) {
+ return (x1 * x1) + (x2 * x2) + (x3 * x3) + (x4 * x4);
+ }
}
diff --git a/commons-geometry-euclidean/src/main/java/org/apache/commons/geometry/euclidean/oned/AffineTransformMatrix1D.java b/commons-geometry-euclidean/src/main/java/org/apache/commons/geometry/euclidean/oned/AffineTransformMatrix1D.java
new file mode 100644
index 0000000..bc67328
--- /dev/null
+++ b/commons-geometry-euclidean/src/main/java/org/apache/commons/geometry/euclidean/oned/AffineTransformMatrix1D.java
@@ -0,0 +1,379 @@
+/*
+ * 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.geometry.euclidean.oned;
+
+import java.io.Serializable;
+
+import org.apache.commons.geometry.core.exception.IllegalNormException;
+import org.apache.commons.geometry.core.internal.DoubleFunction1N;
+import org.apache.commons.geometry.euclidean.exception.NonInvertibleTransformException;
+import org.apache.commons.geometry.euclidean.internal.Vectors;
+import org.apache.commons.numbers.core.Precision;
+
+/** Class using a matrix to represent a affine transformations in 1 dimensional Euclidean space.
+*
+* <p>Instances of this class use a 2x2 matrix for all transform operations.
+* The last row of this matrix is always set to the values <code>[0 1]</code> and so
+* is not stored. Hence, the methods in this class that accept or return arrays always
+* use arrays containing 2 elements, instead of 4.
+* </p>
+*/
+public final class AffineTransformMatrix1D implements Serializable {
+
+ /** Serializable version identifier */
+ private static final long serialVersionUID = 20181006L;
+
+ /** The number of internal matrix elements */
+ private static final int NUM_ELEMENTS = 2;
+
+ /** String used to start the transform matrix string representation */
+ private static final String MATRIX_START = "[ ";
+
+ /** String used to end the transform matrix string representation */
+ private static final String MATRIX_END = " ]";
+
+ /** String used to separate elements in the matrix string representation */
+ private static final String ELEMENT_SEPARATOR = ", ";
+
+ /** Shared transform set to the identity matrix. */
+ private static final AffineTransformMatrix1D IDENTITY_INSTANCE = new AffineTransformMatrix1D(1, 0);
+
+ /** Transform matrix entry <code>m<sub>0,0</sub></code> */
+ private final double m00;
+ /** Transform matrix entry <code>m<sub>0,1</sub></code> */
+ private final double m01;
+
+ /**
+ * Simple constructor; sets all internal matrix elements.
+ * @param m00 matrix entry <code>m<sub>0,0</sub></code>
+ * @param m01 matrix entry <code>m<sub>0,1</sub></code>
+ */
+ private AffineTransformMatrix1D(final double m00, final double m01) {
+ this.m00 = m00;
+ this.m01 = m01;
+ }
+
+ /** Return a 2 element array containing the variable elements from the
+ * internal transformation matrix. The elements are in row-major order.
+ * The array indices map to the internal matrix as follows:
+ * <pre>
+ * [
+ * arr[0], arr[1],
+ * 0 1
+ * ]
+ * </pre>
+ * @return 2 element array containing the variable elements from the
+ * internal transformation matrix
+ */
+ public double[] toArray() {
+ return new double[] {
+ m00, m01
+ };
+ }
+
+ /** Apply this transform to the given vector. A new vector is returned.
+ * @param vec the vector to transform
+ * @return the new, transformed vector
+ */
+ public Vector1D apply(final Vector1D vec) {
+ final double x = vec.getX();
+
+ final double resultX = (m00 * x) + m01;
+
+ return Vector1D.of(resultX);
+ }
+
+ /** Apply this transform to the given vector, ignoring translations.
+ *
+ * <p>This method can be used to transform vector instances representing displacements between points.
+ * For example, if {@code v} represents the difference between points {@code p1} and {@code p2},
+ * then {@code transform.applyVector(v)} will represent the difference between {@code p1} and {@code p2}
+ * after {@code transform} is applied.
+ * </p>
+ *
+ * @param vec the vector to transform
+ * @return the new, transformed vector
+ * @see #applyDirection(Vector1D)
+ */
+ public Vector1D applyVector(final Vector1D vec) {
+ return applyVector(vec, Vector1D::of);
+ }
+
+ /** Apply this transform to the given vector, ignoring translations and normalizing the
+ * result. This is equivalent to {@code transform.applyVector(vec).normalize()} but without
+ * the intermediate vector instance.
+ *
+ * @param vec the vector to transform
+ * @return the new, transformed unit vector
+ * @throws IllegalNormException if the transformed vector coordinates cannot be normalized
+ * @see #applyVector(Vector1D)
+ */
+ public Vector1D applyDirection(final Vector1D vec) {
+ return applyVector(vec, Vector1D::normalize);
+ }
+
+ /** Get a new transform containing the result of applying a translation logically after
+ * the transformation represented by the current instance. This is achieved by
+ * creating a new translation transform and pre-multiplying it with the current
+ * instance. In other words, the returned transform contains the matrix
+ * <code>B * A</code>, where <code>A</code> is the current matrix and <code>B</code>
+ * is the matrix representing the given translation.
+ * @param translation vector containing the translation values for each axis
+ * @return a new transform containing the result of applying a translation to
+ * the current instance
+ */
+ public AffineTransformMatrix1D translate(final Vector1D translation) {
+ return translate(translation.getX());
+ }
+
+ /** Get a new transform containing the result of applying a translation logically after
+ * the transformation represented by the current instance. This is achieved by
+ * creating a new translation transform and pre-multiplying it with the current
+ * instance. In other words, the returned transform contains the matrix
+ * <code>B * A</code>, where <code>A</code> is the current matrix and <code>B</code>
+ * is the matrix representing the given translation.
+ * @param x translation in the x direction
+ * @return a new transform containing the result of applying a translation to
+ * the current instance
+ */
+ public AffineTransformMatrix1D translate(final double x) {
+ return new AffineTransformMatrix1D(m00, m01 + x);
+ }
+
+ /** Get a new transform containing the result of applying a scale operation
+ * logically after the transformation represented by the current instance.
+ * This is achieved by creating a new scale transform and pre-multiplying it with the current
+ * instance. In other words, the returned transform contains the matrix
+ * <code>B * A</code>, where <code>A</code> is the current matrix and <code>B</code>
+ * is the matrix representing the given scale operation.
+ * @param scaleFactor vector containing scale factors for each axis
+ * @return a new transform containing the result of applying a scale operation to
+ * the current instance
+ */
+ public AffineTransformMatrix1D scale(final Vector1D scaleFactor) {
+ return scale(scaleFactor.getX());
+ }
+
+ /** Get a new transform containing the result of applying a scale operation
+ * logically after the transformation represented by the current instance.
+ * This is achieved by creating a new scale transform and pre-multiplying it with the current
+ * instance. In other words, the returned transform contains the matrix
+ * <code>B * A</code>, where <code>A</code> is the current matrix and <code>B</code>
+ * is the matrix representing the given scale operation.
+ * @param x scale factor
+ * @return a new transform containing the result of applying a scale operation to
+ * the current instance
+ */
+ public AffineTransformMatrix1D scale(final double x) {
+ return new AffineTransformMatrix1D(m00 * x, m01 * x);
+ }
+
+ /** Get a new transform created by multiplying this instance by the argument.
+ * This is equivalent to the expression {@code A * M} where {@code A} is the
+ * current transform matrix and {@code M} is the given transform matrix. In
+ * terms of transformations, applying the returned matrix is equivalent to
+ * applying {@code M} and <em>then</em> applying {@code A}. In other words,
+ * the rightmost transform is applied first.
+ *
+ * @param m the transform to multiply with
+ * @return the result of multiplying the current instance by the given
+ * transform matrix
+ */
+ public AffineTransformMatrix1D multiply(final AffineTransformMatrix1D m) {
+ return multiply(this, m);
+ }
+
+ /** Get a new transform created by multiplying the argument by this instance.
+ * This is equivalent to the expression {@code M * A} where {@code A} is the
+ * current transform matrix and {@code M} is the given transform matrix. In
+ * terms of transformations, applying the returned matrix is equivalent to
+ * applying {@code A} and <em>then</em> applying {@code M}. In other words,
+ * the rightmost transform is applied first.
+ *
+ * @param m the transform to multiply with
+ * @return the result of multiplying the given transform matrix by the current
+ * instance
+ */
+ public AffineTransformMatrix1D premultiply(final AffineTransformMatrix1D m) {
+ return multiply(m, this);
+ }
+
+ /** Get a new transform representing the inverse of the current instance.
+ * @return inverse transform
+ * @throws NonInvertibleTransformException if the transform matrix cannot be inverted
+ */
+ public AffineTransformMatrix1D getInverse() {
+
+ final double det = this.m00;
+
+ if (!Vectors.isRealNonZero(det)) {
+ throw new NonInvertibleTransformException("Transform is not invertible; matrix determinant is " + det);
+ }
+
+ validateElementForInverse(m01);
+
+ final double invDet = 1.0 / det;
+
+ final double c00 = invDet;
+ final double c01 = - (this.m01 * invDet);
+
+ return new AffineTransformMatrix1D(c00, c01);
+ }
+
+ /** {@inheritDoc} */
+ @Override
+ public int hashCode() {
+ final int prime = 31;
+ int result = 1;
+
+ result = (result * prime) + Double.hashCode(m00);
+ result = (result * prime) + Double.hashCode(m01);
+
+ return result;
+ }
+
+ /**
+ * Return true if the given object is an instance of {@link AffineTransformMatrix1D}
+ * and all matrix element values are exactly equal.
+ * @param obj object to test for equality with the current instance
+ * @return true if all transform matrix elements are exactly equal; otherwise false
+ */
+ @Override
+ public boolean equals(Object obj) {
+ if (this == obj) {
+ return true;
+ }
+ if (!(obj instanceof AffineTransformMatrix1D)) {
+ return false;
+ }
+ final AffineTransformMatrix1D other = (AffineTransformMatrix1D) obj;
+
+ return Precision.equals(this.m00, other.m00) &&
+ Precision.equals(this.m01, other.m01);
+ }
+
+ /** {@inheritDoc} */
+ @Override
+ public String toString() {
+ final StringBuilder sb = new StringBuilder();
+
+ sb.append(MATRIX_START)
+
+ .append(m00)
+ .append(ELEMENT_SEPARATOR)
+ .append(m01)
+
+ .append(MATRIX_END);
+
+ return sb.toString();
+ }
+
+ /** Multiplies the given vector by the scaling component of this transform.
+ * The computed coordinate is passed to the given factory function.
+ * @param <T> factory output type
+ * @param vec the vector to transform
+ * @param factory the factory instance that will be passed the transformed coordinate
+ * @return the factory return value
+ */
+ private <T> T applyVector(final Vector1D vec, final DoubleFunction1N<T> factory) {
+ final double resultX = m00 * vec.getX();
+
+ return factory.apply(resultX);
+ }
+
+ /** Get a new transform with the given matrix elements. The array must contain 2 elements.
+ * The first element in the array represents the scale factor for the transform and the
+ * second represents the translation.
+ * @param arr 2-element array containing values for the variable entries in the
+ * transform matrix
+ * @return a new transform initialized with the given matrix values
+ * @throws IllegalArgumentException if the array does not have 2 elements
+ */
+ public static AffineTransformMatrix1D of(final double ... arr) {
+ if (arr.length != NUM_ELEMENTS) {
+ throw new IllegalArgumentException("Dimension mismatch: " + arr.length + " != " + NUM_ELEMENTS);
+ }
+
+ return new AffineTransformMatrix1D(arr[0], arr[1]);
+ }
+
+ /** Get the transform representing the identity matrix. This transform does not
+ * modify point or vector values when applied.
+ * @return transform representing the identity matrix
+ */
+ public static AffineTransformMatrix1D identity() {
+ return IDENTITY_INSTANCE;
+ }
+
+ /** Get a transform representing the given translation.
+ * @param translation vector containing translation values for each axis
+ * @return a new transform representing the given translation
+ */
+ public static AffineTransformMatrix1D createTranslation(final Vector1D translation) {
+ return createTranslation(translation.getX());
+ }
+
+ /** Get a transform representing the given translation.
+ * @param x translation in the x direction
+ * @return a new transform representing the given translation
+ */
+ public static AffineTransformMatrix1D createTranslation(final double x) {
+ return new AffineTransformMatrix1D(1, x);
+ }
+
+ /** Get a transform representing a scale operation.
+ * @param factor vector containing the scale factor
+ * @return a new transform representing a scale operation
+ */
+ public static AffineTransformMatrix1D createScale(final Vector1D factor) {
+ return createScale(factor.getX());
+ }
+
+ /** Get a transform representing a scale operation.
+ * @param factor scale factor
+ * @return a new transform representing a scale operation
+ */
+ public static AffineTransformMatrix1D createScale(final double factor) {
+ return new AffineTransformMatrix1D(factor, 0);
+ }
+
+ /** Multiply two transform matrices together.
+ * @param a first transform
+ * @param b second transform
+ * @return the transform computed as {@code a x b}
+ */
+ private static AffineTransformMatrix1D multiply(final AffineTransformMatrix1D a, final AffineTransformMatrix1D b) {
+
+ // calculate the matrix elements
+ final double c00 = a.m00 * b.m00;
+ final double c01 = (a.m00 * b.m01) + a.m01;
+
+ return new AffineTransformMatrix1D(c00, c01);
+ }
+
+ /** Checks that the given matrix element is valid for use in calculation of
+ * a matrix inverse. Throws a {@link NonInvertibleTransformException} if not.
+ * @param element matrix entry to check
+ * @throws NonInvertibleTransformException if the element is not valid for use
+ * in calculating a matrix inverse, ie if it is NaN or infinite.
+ */
+ private static void validateElementForInverse(final double element) {
+ if (!Double.isFinite(element)) {
+ throw new NonInvertibleTransformException("Transform is not invertible; invalid matrix element: " + element);
+ }
+ }
+}
diff --git a/commons-geometry-euclidean/src/main/java/org/apache/commons/geometry/euclidean/oned/Vector1D.java b/commons-geometry-euclidean/src/main/java/org/apache/commons/geometry/euclidean/oned/Vector1D.java
index da4ce11..dc3b741 100644
--- a/commons-geometry-euclidean/src/main/java/org/apache/commons/geometry/euclidean/oned/Vector1D.java
+++ b/commons-geometry-euclidean/src/main/java/org/apache/commons/geometry/euclidean/oned/Vector1D.java
@@ -209,6 +209,16 @@ public class Vector1D extends EuclideanVector<Vector1D> {
return (sig1 == sig2) ? 0.0 : Geometry.PI;
}
+ /** Apply the given transform to this vector, returning the result as a
+ * new vector instance.
+ * @param transform the transform to apply
+ * @return a new, transformed vector
+ * @see AffineTransformMatrix1D#apply(Vector1D)
+ */
+ public Vector1D transform(AffineTransformMatrix1D transform) {
+ return transform.apply(this);
+ }
+
/**
* Get a hashCode for the vector.
* <p>All NaN values have the same hash code.</p>
diff --git a/commons-geometry-euclidean/src/main/java/org/apache/commons/geometry/euclidean/threed/AffineTransformMatrix3D.java b/commons-geometry-euclidean/src/main/java/org/apache/commons/geometry/euclidean/threed/AffineTransformMatrix3D.java
new file mode 100644
index 0000000..1c98b34
--- /dev/null
+++ b/commons-geometry-euclidean/src/main/java/org/apache/commons/geometry/euclidean/threed/AffineTransformMatrix3D.java
@@ -0,0 +1,629 @@
+/*
+ * 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.geometry.euclidean.threed;
+
+import java.io.Serializable;
+
+import org.apache.commons.geometry.core.exception.IllegalNormException;
+import org.apache.commons.geometry.core.internal.DoubleFunction3N;
+import org.apache.commons.geometry.euclidean.exception.NonInvertibleTransformException;
+import org.apache.commons.geometry.euclidean.internal.Matrices;
+import org.apache.commons.geometry.euclidean.internal.Vectors;
+import org.apache.commons.geometry.euclidean.threed.rotation.QuaternionRotation;
+import org.apache.commons.numbers.arrays.LinearCombination;
+import org.apache.commons.numbers.core.Precision;
+
+/** Class using a matrix to represent affine transformations in 3 dimensional Euclidean space.
+ *
+ * <p>Instances of this class use a 4x4 matrix for all transform operations.
+ * The last row of this matrix is always set to the values <code>[0 0 0 1]</code> and so
+ * is not stored. Hence, the methods in this class that accept or return arrays always
+ * use arrays containing 12 elements, instead of 16.
+ * </p>
+ */
+public final class AffineTransformMatrix3D implements Serializable {
+
+ /** Serializable version identifier */
+ private static final long serialVersionUID = 20180923L;
+
+ /** The number of internal matrix elements */
+ private static final int NUM_ELEMENTS = 12;
+
+ /** String used to start the transform matrix string representation */
+ private static final String MATRIX_START = "[ ";
+
+ /** String used to end the transform matrix string representation */
+ private static final String MATRIX_END = " ]";
+
+ /** String used to separate elements in the matrix string representation */
+ private static final String ELEMENT_SEPARATOR = ", ";
+
+ /** String used to separate rows in the matrix string representation */
+ private static final String ROW_SEPARATOR = "; ";
+
+ /** Shared transform set to the identity matrix. */
+ private static final AffineTransformMatrix3D IDENTITY_INSTANCE = new AffineTransformMatrix3D(
+ 1, 0, 0, 0,
+ 0, 1, 0, 0,
+ 0, 0, 1, 0
+ );
+
+ /** Transform matrix entry <code>m<sub>0,0</sub></code> */
+ private final double m00;
+ /** Transform matrix entry <code>m<sub>0,1</sub></code> */
+ private final double m01;
+ /** Transform matrix entry <code>m<sub>0,2</sub></code> */
+ private final double m02;
+ /** Transform matrix entry <code>m<sub>0,3</sub></code> */
+ private final double m03;
+
+ /** Transform matrix entry <code>m<sub>1,0</sub></code> */
+ private final double m10;
+ /** Transform matrix entry <code>m<sub>1,1</sub></code> */
+ private final double m11;
+ /** Transform matrix entry <code>m<sub>1,2</sub></code> */
+ private final double m12;
+ /** Transform matrix entry <code>m<sub>1,3</sub></code> */
+ private final double m13;
+
+ /** Transform matrix entry <code>m<sub>2,0</sub></code> */
+ private final double m20;
+ /** Transform matrix entry <code>m<sub>2,1</sub></code> */
+ private final double m21;
+ /** Transform matrix entry <code>m<sub>2,2</sub></code> */
+ private final double m22;
+ /** Transform matrix entry <code>m<sub>2,3</sub></code> */
+ private final double m23;
+
+ /**
+ * Package-private constructor; sets all internal matrix elements.
+ * @param m00 matrix entry <code>m<sub>0,0</sub></code>
+ * @param m01 matrix entry <code>m<sub>0,1</sub></code>
+ * @param m02 matrix entry <code>m<sub>0,2</sub></code>
+ * @param m03 matrix entry <code>m<sub>0,3</sub></code>
+ * @param m10 matrix entry <code>m<sub>1,0</sub></code>
+ * @param m11 matrix entry <code>m<sub>1,1</sub></code>
+ * @param m12 matrix entry <code>m<sub>1,2</sub></code>
+ * @param m13 matrix entry <code>m<sub>1,3</sub></code>
+ * @param m20 matrix entry <code>m<sub>2,0</sub></code>
+ * @param m21 matrix entry <code>m<sub>2,1</sub></code>
+ * @param m22 matrix entry <code>m<sub>2,2</sub></code>
+ * @param m23 matrix entry <code>m<sub>2,3</sub></code>
+ */
+ AffineTransformMatrix3D(
+ final double m00, final double m01, final double m02, final double m03,
+ final double m10, final double m11, final double m12, final double m13,
+ final double m20, final double m21, final double m22, final double m23) {
+
+ this.m00 = m00;
+ this.m01 = m01;
+ this.m02 = m02;
+ this.m03 = m03;
+
+ this.m10 = m10;
+ this.m11 = m11;
+ this.m12 = m12;
+ this.m13 = m13;
+
+ this.m20 = m20;
+ this.m21 = m21;
+ this.m22 = m22;
+ this.m23 = m23;
+ }
+
+ /** Return a 12 element array containing the variable elements from the
+ * internal transformation matrix. The elements are in row-major order.
+ * The array indices map to the internal matrix as follows:
+ * <pre>
+ * [
+ * arr[0], arr[1], arr[2], arr[3]
+ * arr[4], arr[5], arr[6], arr[7],
+ * arr[8], arr[9], arr[10], arr[11],
+ * 0 0 0 1
+ * ]
+ * </pre>
+ * @return 12 element array containing the variable elements from the
+ * internal transformation matrix
+ */
+ public double[] toArray() {
+ return new double[] {
+ m00, m01, m02, m03,
+ m10, m11, m12, m13,
+ m20, m21, m22, m23
+ };
+ }
+
+ /** Apply this transform to the given vector, returning the result as a new instance.
+ *
+ * <p>The transformed vector is computed by creating a 4-element column vector from the
+ * coordinates in the input and setting the last element to 1. This is then multiplied with the
+ * 4x4 transform matrix to produce the transformed vector. The {@code 1} in the last position
+ * is ignored.
+ * <pre>
+ * [ m00 m01 m02 m03 ] [ x ] [ x']
+ * [ m10 m11 m12 m13 ] * [ y ] = [ y']
+ * [ m20 m21 m22 m23 ] [ z ] [ z']
+ * [ 0 0 0 1 ] [ 1 ] [ 1 ]
+ * </pre>
+ *
+ * @param vec the vector to transform
+ * @return the new, transformed vector
+ */
+ public Vector3D apply(final Vector3D vec) {
+ final double x = vec.getX();
+ final double y = vec.getY();
+ final double z = vec.getZ();
+
+ final double resultX = LinearCombination.value(m00, x, m01, y, m02, z) + m03;
+ final double resultY = LinearCombination.value(m10, x, m11, y, m12, z) + m13;
+ final double resultZ = LinearCombination.value(m20, x, m21, y, m22, z) + m23;
+
+ return Vector3D.of(resultX, resultY, resultZ);
+ }
+
+ /** Apply this transform to the given vector, ignoring translations.
+ *
+ * <p>This method can be used to transform vector instances representing displacements between points.
+ * For example, if {@code v} represents the difference between points {@code p1} and {@code p2},
+ * then {@code transform.applyVector(v)} will represent the difference between {@code p1} and {@code p2}
+ * after {@code transform} is applied.
+ * </p>
+ *
+ * <p>The transformed vector is computed by creating a 4-element column vector from the
+ * coordinates in the input and setting the last element to 0. This is then multiplied with the
+ * 4x4 transform matrix to produce the transformed vector. The {@code 0} in the last position
+ * is ignored.
+ * <pre>
+ * [ m00 m01 m02 m03 ] [ x ] [ x']
+ * [ m10 m11 m12 m13 ] * [ y ] = [ y']
+ * [ m20 m21 m22 m23 ] [ z ] [ z']
+ * [ 0 0 0 1 ] [ 0 ] [ 0 ]
+ * </pre>
+ *
+ * @param vec the vector to transform
+ * @return the new, transformed vector
+ * @see #applyDirection(Vector3D)
+ */
+ public Vector3D applyVector(final Vector3D vec) {
+ return applyVector(vec, Vector3D::of);
+ }
+
+ /** Apply this transform to the given vector, ignoring translations and normalizing the
+ * result. This is equivalent to {@code transform.applyVector(vec).normalize()} but without
+ * the intermediate vector instance.
+ * @param vec the vector to transform
+ * @return the new, transformed unit vector
+ * @throws IllegalNormException if the transformed vector coordinates cannot be normalized
+ * @see #applyVector(Vector3D)
+ */
+ public Vector3D applyDirection(final Vector3D vec) {
+ return applyVector(vec, Vector3D::normalize);
+ }
+
+ /** Apply a translation to the current instance, returning the result as a new transform.
+ * @param translation vector containing the translation values for each axis
+ * @return a new transform containing the result of applying a translation to
+ * the current instance
+ */
+ public AffineTransformMatrix3D translate(final Vector3D translation) {
+ return translate(translation.getX(), translation.getY(), translation.getZ());
+ }
+
+ /** Apply a translation to the current instance, returning the result as a new transform.
+ * @param x translation in the x direction
+ * @param y translation in the y direction
+ * @param z translation in the z direction
+ * @return a new transform containing the result of applying a translation to
+ * the current instance
+ */
+ public AffineTransformMatrix3D translate(final double x, final double y, final double z) {
+ return new AffineTransformMatrix3D(
+ m00, m01, m02, m03 + x,
+ m10, m11, m12, m13 + y,
+ m20, m21, m22, m23 + z
+ );
+ }
+
+ /** Apply a scale operation to the current instance, returning the result as a new transform.
+ * @param factor the scale factor to apply to all axes
+ * @return a new transform containing the result of applying a scale operation to
+ * the current instance
+ */
+ public AffineTransformMatrix3D scale(final double factor) {
+ return scale(factor, factor, factor);
+ }
+
+ /** Apply a scale operation to the current instance, returning the result as a new transform.
+ * @param scaleFactors vector containing scale factors for each axis
+ * @return a new transform containing the result of applying a scale operation to
+ * the current instance
+ */
+ public AffineTransformMatrix3D scale(final Vector3D scaleFactors) {
+ return scale(scaleFactors.getX(), scaleFactors.getY(), scaleFactors.getZ());
+ }
+
+ /** Apply a scale operation to the current instance, returning the result as a new transform.
+ * @param x scale factor for the x axis
+ * @param y scale factor for the y axis
+ * @param z scale factor for the z axis
+ * @return a new transform containing the result of applying a scale operation to
+ * the current instance
+ */
+ public AffineTransformMatrix3D scale(final double x, final double y, final double z) {
+ return new AffineTransformMatrix3D(
+ m00 * x, m01 * x, m02 * x, m03 * x,
+ m10 * y, m11 * y, m12 * y, m13 * y,
+ m20 * z, m21 * z, m22 * z, m23 * z
+ );
+ }
+
+ /** Apply a rotation to the current instance, returning the result as a new transform.
+ * @param rotation the rotation to apply
+ * @return a new transform containing the result of applying a rotation to the
+ * current instance
+ * @see QuaternionRotation#toTransformMatrix()
+ */
+ public AffineTransformMatrix3D rotate(final QuaternionRotation rotation) {
+ return multiply(rotation.toTransformMatrix(), this);
+ }
+
+ /** Apply a rotation around the given center point to the current instance, returning the result
+ * as a new transform. This is achieved by translating the center point to the origin, applying
+ * the rotation, and then translating back.
+ * @param center the center of rotation
+ * @param rotation the rotation to apply
+ * @return a new transform containing the result of applying a rotation about the given center
+ * point to the current instance
+ * @see QuaternionRotation#toTransformMatrix()
+ */
+ public AffineTransformMatrix3D rotate(final Vector3D center, final QuaternionRotation rotation) {
+ return multiply(createRotation(center, rotation), this);
+ }
+
+ /** Get a new transform created by multiplying this instance by the argument.
+ * This is equivalent to the expression {@code A * M} where {@code A} is the
+ * current transform matrix and {@code M} is the given transform matrix. In
+ * terms of transformations, applying the returned matrix is equivalent to
+ * applying {@code M} and <em>then</em> applying {@code A}. In other words,
+ * the rightmost transform is applied first.
+ *
+ * @param m the transform to multiply with
+ * @return the result of multiplying the current instance by the given
+ * transform matrix
+ */
+ public AffineTransformMatrix3D multiply(final AffineTransformMatrix3D m) {
+ return multiply(this, m);
+ }
+
+ /** Get a new transform created by multiplying the argument by this instance.
+ * This is equivalent to the expression {@code M * A} where {@code A} is the
+ * current transform matrix and {@code M} is the given transform matrix. In
+ * terms of transformations, applying the returned matrix is equivalent to
+ * applying {@code A} and <em>then</em> applying {@code M}. In other words,
+ * the rightmost transform is applied first.
+ *
+ * @param m the transform to multiply with
+ * @return the result of multiplying the given transform matrix by the current
+ * instance
+ */
+ public AffineTransformMatrix3D premultiply(final AffineTransformMatrix3D m) {
+ return multiply(m, this);
+ }
+
+ /** Get a new transform representing the inverse of the current instance.
+ * @return inverse transform
+ * @throws NonInvertibleTransformException if the transform matrix cannot be inverted
+ */
+ public AffineTransformMatrix3D getInverse() {
+
+ // Our full matrix is 4x4 but we can significantly reduce the amount of computations
+ // needed here since we know that our last row is [0 0 0 1].
+
+ // compute the determinant of the matrix
+ final double det = Matrices.determinant(
+ m00, m01, m02,
+ m10, m11, m12,
+ m20, m21, m22
+ );
+
+ if (!Vectors.isRealNonZero(det)) {
+ throw new NonInvertibleTransformException("Transform is not invertible; matrix determinant is " + det);
+ }
+
+ // validate the remaining matrix elements that were not part of the determinant
+ validateElementForInverse(m03);
+ validateElementForInverse(m13);
+ validateElementForInverse(m23);
+
+ // compute the necessary elements of the cofactor matrix
+ // (we need all but the last column)
+
+ final double invDet = 1.0 / det;
+
+ final double c00 = invDet * Matrices.determinant(m11, m12, m21, m22);
+ final double c01 = - invDet * Matrices.determinant(m10, m12, m20, m22);
+ final double c02 = invDet * Matrices.determinant(m10, m11, m20, m21);
+
+ final double c10 = - invDet * Matrices.determinant(m01, m02, m21, m22);
+ final double c11 = invDet * Matrices.determinant(m00, m02, m20, m22);
+ final double c12 = - invDet * Matrices.determinant(m00, m01, m20, m21);
+
+ final double c20 = invDet * Matrices.determinant(m01, m02, m11, m12);
+ final double c21 = - invDet * Matrices.determinant(m00, m02, m10, m12);
+ final double c22 = invDet * Matrices.determinant(m00, m01, m10, m11);
+
+ final double c30 = - invDet * Matrices.determinant(
+ m01, m02, m03,
+ m11, m12, m13,
+ m21, m22, m23
+ );
+ final double c31 = invDet * Matrices.determinant(
+ m00, m02, m03,
+ m10, m12, m13,
+ m20, m22, m23
+ );
+ final double c32 = - invDet * Matrices.determinant(
+ m00, m01, m03,
+ m10, m11, m13,
+ m20, m21, m23
+ );
+
+ return new AffineTransformMatrix3D(
+ c00, c10, c20, c30,
+ c01, c11, c21, c31,
+ c02, c12, c22, c32
+ );
+ }
+
+ /** {@inheritDoc} */
+ @Override
+ public int hashCode() {
+ final int prime = 31;
+ int result = 1;
+
+ result = (result * prime) + (Double.hashCode(m00) - Double.hashCode(m01) + Double.hashCode(m02) - Double.hashCode(m03));
+ result = (result * prime) + (Double.hashCode(m10) - Double.hashCode(m11) + Double.hashCode(m12) - Double.hashCode(m13));
+ result = (result * prime) + (Double.hashCode(m20) - Double.hashCode(m21) + Double.hashCode(m22) - Double.hashCode(m23));
+
+ return result;
+ }
+
+ /**
+ * Return true if the given object is an instance of {@link AffineTransformMatrix3D}
+ * and all matrix element values are exactly equal.
+ * @param obj object to test for equality with the current instance
+ * @return true if all transform matrix elements are exactly equal; otherwise false
+ */
+ @Override
+ public boolean equals(Object obj) {
+ if (this == obj) {
+ return true;
+ }
+ if (!(obj instanceof AffineTransformMatrix3D)) {
+ return false;
+ }
+
+ final AffineTransformMatrix3D other = (AffineTransformMatrix3D) obj;
+
+ return Precision.equals(this.m00, other.m00) &&
+ Precision.equals(this.m01, other.m01) &&
+ Precision.equals(this.m02, other.m02) &&
+ Precision.equals(this.m03, other.m03) &&
+
+ Precision.equals(this.m10, other.m10) &&
+ Precision.equals(this.m11, other.m11) &&
+ Precision.equals(this.m12, other.m12) &&
+ Precision.equals(this.m13, other.m13) &&
+
+ Precision.equals(this.m20, other.m20) &&
+ Precision.equals(this.m21, other.m21) &&
+ Precision.equals(this.m22, other.m22) &&
+ Precision.equals(this.m23, other.m23);
+ }
+
+ /** {@inheritDoc} */
+ @Override
+ public String toString() {
+ final StringBuilder sb = new StringBuilder();
+
+ sb.append(MATRIX_START)
+
+ .append(m00)
+ .append(ELEMENT_SEPARATOR)
+ .append(m01)
+ .append(ELEMENT_SEPARATOR)
+ .append(m02)
+ .append(ELEMENT_SEPARATOR)
+ .append(m03)
+ .append(ROW_SEPARATOR)
+
+ .append(m10)
+ .append(ELEMENT_SEPARATOR)
+ .append(m11)
+ .append(ELEMENT_SEPARATOR)
+ .append(m12)
+ .append(ELEMENT_SEPARATOR)
+ .append(m13)
+ .append(ROW_SEPARATOR)
+
+ .append(m20)
+ .append(ELEMENT_SEPARATOR)
+ .append(m21)
+ .append(ELEMENT_SEPARATOR)
+ .append(m22)
+ .append(ELEMENT_SEPARATOR)
+ .append(m23)
+
+ .append(MATRIX_END);
+
+ return sb.toString();
+ }
+
+ /** Multiplies the given vector by the 3x3 linear transformation matrix contained in the
+ * upper-right corner of the affine transformation matrix. This applies all transformation
+ * operations except for translations. The computed coordinates are passed to the given
+ * factory function.
+ * @param <T> factory output type
+ * @param vec the vector to transform
+ * @param factory the factory instance that will be passed the transformed coordinates
+ * @return the factory return value
+ */
+ private <T> T applyVector(final Vector3D vec, final DoubleFunction3N<T> factory) {
+ final double x = vec.getX();
+ final double y = vec.getY();
+ final double z = vec.getZ();
+
+ final double resultX = LinearCombination.value(m00, x, m01, y, m02, z);
+ final double resultY = LinearCombination.value(m10, x, m11, y, m12, z);
+ final double resultZ = LinearCombination.value(m20, x, m21, y, m22, z);
+
+ return factory.apply(resultX, resultY, resultZ);
+ }
+
+ /** Get a new transform with the given matrix elements. The array must contain 12 elements.
+ * @param arr 12-element array containing values for the variable entries in the
+ * transform matrix
+ * @return a new transform initialized with the given matrix values
+ * @throws IllegalArgumentException if the array does not have 12 elements
+ */
+ public static AffineTransformMatrix3D of(final double ... arr) {
+ if (arr.length != NUM_ELEMENTS) {
+ throw new IllegalArgumentException("Dimension mismatch: " + arr.length + " != " + NUM_ELEMENTS);
+ }
+
+ return new AffineTransformMatrix3D(
+ arr[0], arr[1], arr[2], arr[3],
+ arr[4], arr[5], arr[6], arr[7],
+ arr[8], arr[9], arr[10], arr[11]
+ );
+ }
+
+ /** Get the transform representing the identity matrix. This transform does not
+ * modify point or vector values when applied.
+ * @return transform representing the identity matrix
+ */
+ public static AffineTransformMatrix3D identity() {
+ return IDENTITY_INSTANCE;
+ }
+
+ /** Create a transform representing the given translation.
+ * @param translation vector containing translation values for each axis
+ * @return a new transform representing the given translation
+ */
+ public static AffineTransformMatrix3D createTranslation(final Vector3D translation) {
+ return createTranslation(translation.getX(), translation.getY(), translation.getZ());
+ }
+
+ /** Create a transform representing the given translation.
+ * @param x translation in the x direction
+ * @param y translation in the y direction
+ * @param z translation in the z direction
+ * @return a new transform representing the given translation
+ */
+ public static AffineTransformMatrix3D createTranslation(final double x, final double y, final double z) {
+ return new AffineTransformMatrix3D(
+ 1, 0, 0, x,
+ 0, 1, 0, y,
+ 0, 0, 1, z
+ );
+ }
+
+ /** Create a transform representing a scale operation with the given scale factor applied to all axes.
+ * @param factor scale factor to apply to all axes
+ * @return a new transform representing a uniform scaling in all axes
+ */
+ public static AffineTransformMatrix3D createScale(final double factor) {
+ return createScale(factor, factor, factor);
+ }
+
+ /** Create a transform representing a scale operation.
+ * @param factors vector containing scale factors for each axis
+ * @return a new transform representing a scale operation
+ */
+ public static AffineTransformMatrix3D createScale(final Vector3D factors) {
+ return createScale(factors.getX(), factors.getY(), factors.getZ());
+ }
+
+ /** Create a transform representing a scale operation.
+ * @param x scale factor for the x axis
+ * @param y scale factor for the y axis
+ * @param z scale factor for the z axis
+ * @return a new transform representing a scale operation
+ */
+ public static AffineTransformMatrix3D createScale(final double x, final double y, final double z) {
+ return new AffineTransformMatrix3D(
+ x, 0, 0, 0,
+ 0, y, 0, 0,
+ 0, 0, z, 0
+ );
+ }
+
+ /** Create a transform representing a rotation about the given center point. This is achieved by translating
+ * the center to the origin, applying the rotation, and then translating back.
+ * @param center the center of rotation
+ * @param rotation the rotation to apply
+ * @return a new transform representing a rotation about the given center point
+ * @see QuaternionRotation#toTransformMatrix()
+ */
+ public static AffineTransformMatrix3D createRotation(final Vector3D center, final QuaternionRotation rotation) {
+ return createTranslation(center.negate())
+ .rotate(rotation)
+ .translate(center);
+ }
+
+ /** Multiply two transform matrices together and return the result as a new transform instance.
+ * @param a first transform
+ * @param b second transform
+ * @return the transform computed as {@code a x b}
+ */
+ private static AffineTransformMatrix3D multiply(final AffineTransformMatrix3D a, final AffineTransformMatrix3D b) {
+
+ // calculate the matrix elements
+ final double c00 = LinearCombination.value(a.m00, b.m00, a.m01, b.m10, a.m02, b.m20);
+ final double c01 = LinearCombination.value(a.m00, b.m01, a.m01, b.m11, a.m02, b.m21);
+ final double c02 = LinearCombination.value(a.m00, b.m02, a.m01, b.m12, a.m02, b.m22);
+ final double c03 = LinearCombination.value(a.m00, b.m03, a.m01, b.m13, a.m02, b.m23) + a.m03;
+
+ final double c10 = LinearCombination.value(a.m10, b.m00, a.m11, b.m10, a.m12, b.m20);
+ final double c11 = LinearCombination.value(a.m10, b.m01, a.m11, b.m11, a.m12, b.m21);
+ final double c12 = LinearCombination.value(a.m10, b.m02, a.m11, b.m12, a.m12, b.m22);
+ final double c13 = LinearCombination.value(a.m10, b.m03, a.m11, b.m13, a.m12, b.m23) + a.m13;
+
+ final double c20 = LinearCombination.value(a.m20, b.m00, a.m21, b.m10, a.m22, b.m20);
+ final double c21 = LinearCombination.value(a.m20, b.m01, a.m21, b.m11, a.m22, b.m21);
+ final double c22 = LinearCombination.value(a.m20, b.m02, a.m21 , b.m12, a.m22, b.m22);
+ final double c23 = LinearCombination.value(a.m20, b.m03, a.m21 , b.m13, a.m22, b.m23) + a.m23;
+
+ return new AffineTransformMatrix3D(
+ c00, c01, c02, c03,
+ c10, c11, c12, c13,
+ c20, c21, c22, c23
+ );
+ }
+
+ /** Checks that the given matrix element is valid for use in calculation of
+ * a matrix inverse. Throws a {@link NonInvertibleTransformException} if not.
+ * @param element matrix entry to check
+ * @throws NonInvertibleTransformException if the element is not valid for use
+ * in calculating a matrix inverse, ie if it is NaN or infinite.
+ */
+ private static void validateElementForInverse(final double element) {
+ if (!Double.isFinite(element)) {
+ throw new NonInvertibleTransformException("Transform is not invertible; invalid matrix element: " + element);
+ }
+ }
+}
diff --git a/commons-geometry-euclidean/src/main/java/org/apache/commons/geometry/euclidean/threed/Plane.java b/commons-geometry-euclidean/src/main/java/org/apache/commons/geometry/euclidean/threed/Plane.java
index 865b666..8e97a64 100644
--- a/commons-geometry-euclidean/src/main/java/org/apache/commons/geometry/euclidean/threed/Plane.java
+++ b/commons-geometry-euclidean/src/main/java/org/apache/commons/geometry/euclidean/threed/Plane.java
@@ -21,8 +21,9 @@ import org.apache.commons.geometry.core.partitioning.Embedding;
import org.apache.commons.geometry.core.partitioning.Hyperplane;
import org.apache.commons.geometry.euclidean.internal.Vectors;
import org.apache.commons.geometry.euclidean.oned.Vector1D;
-import org.apache.commons.geometry.euclidean.twod.Vector2D;
+import org.apache.commons.geometry.euclidean.threed.rotation.QuaternionRotation;
import org.apache.commons.geometry.euclidean.twod.PolygonsSet;
+import org.apache.commons.geometry.euclidean.twod.Vector2D;
/** The class represent planes in a three dimensional space.
*/
@@ -281,18 +282,18 @@ public class Plane implements Hyperplane<Vector3D>, Embedding<Vector3D, Vector2D
/** Rotate the plane around the specified point.
* <p>The instance is not modified, a new instance is created.</p>
* @param center rotation center
- * @param rotation vectorial rotation operator
+ * @param rotation 3-dimensional rotation
* @return a new plane
*/
- public Plane rotate(final Vector3D center, final Rotation rotation) {
+ public Plane rotate(final Vector3D center, final QuaternionRotation rotation) {
final Vector3D delta = origin.subtract(center);
- final Plane plane = new Plane(center.add(rotation.applyTo(delta)),
- rotation.applyTo(w), tolerance);
+ final Plane plane = new Plane(center.add(rotation.apply(delta)),
+ rotation.apply(w), tolerance);
// make sure the frame is transformed as desired
- plane.u = rotation.applyTo(u);
- plane.v = rotation.applyTo(v);
+ plane.u = rotation.apply(u);
+ plane.v = rotation.apply(v);
return plane;
diff --git a/commons-geometry-euclidean/src/main/java/org/apache/commons/geometry/euclidean/threed/PolyhedronsSet.java b/commons-geometry-euclidean/src/main/java/org/apache/commons/geometry/euclidean/threed/PolyhedronsSet.java
index c107952..fd7ded9 100644
--- a/commons-geometry-euclidean/src/main/java/org/apache/commons/geometry/euclidean/threed/PolyhedronsSet.java
+++ b/commons-geometry-euclidean/src/main/java/org/apache/commons/geometry/euclidean/threed/PolyhedronsSet.java
@@ -33,6 +33,7 @@ import org.apache.commons.geometry.core.partitioning.RegionFactory;
import org.apache.commons.geometry.core.partitioning.SubHyperplane;
import org.apache.commons.geometry.core.partitioning.Transform;
import org.apache.commons.geometry.euclidean.oned.Vector1D;
+import org.apache.commons.geometry.euclidean.threed.rotation.QuaternionRotation;
import org.apache.commons.geometry.euclidean.twod.Vector2D;
import org.apache.commons.geometry.euclidean.twod.PolygonsSet;
import org.apache.commons.geometry.euclidean.twod.SubLine;
@@ -560,10 +561,10 @@ public class PolyhedronsSet extends AbstractRegion<Vector3D, Vector2D> {
/** Rotate the region around the specified point.
* <p>The instance is not modified, a new instance is created.</p>
* @param center rotation center
- * @param rotation vectorial rotation operator
+ * @param rotation 3-dimensional rotation
* @return a new instance representing the rotated region
*/
- public PolyhedronsSet rotate(final Vector3D center, final Rotation rotation) {
+ public PolyhedronsSet rotate(final Vector3D center, final QuaternionRotation rotation) {
return (PolyhedronsSet) applyTransform(new RotationTransform(center, rotation));
}
@@ -573,8 +574,8 @@ public class PolyhedronsSet extends AbstractRegion<Vector3D, Vector2D> {
/** Center point of the rotation. */
private final Vector3D center;
- /** Vectorial rotation. */
- private final Rotation rotation;
+ /** Quaternion rotation. */
+ private final QuaternionRotation rotation;
/** Cached original hyperplane. */
private Plane cachedOriginal;
@@ -586,7 +587,7 @@ public class PolyhedronsSet extends AbstractRegion<Vector3D, Vector2D> {
* @param center center point of the rotation
* @param rotation vectorial rotation
*/
- RotationTransform(final Vector3D center, final Rotation rotation) {
+ RotationTransform(final Vector3D center, final QuaternionRotation rotation) {
this.center = center;
this.rotation = rotation;
}
@@ -595,7 +596,7 @@ public class PolyhedronsSet extends AbstractRegion<Vector3D, Vector2D> {
@Override
public Vector3D apply(final Vector3D point) {
final Vector3D delta = point.subtract(center);
- return Vector3D.linearCombination(1.0, center, 1.0, rotation.applyTo(delta));
+ return Vector3D.linearCombination(1.0, center, 1.0, rotation.apply(delta));
}
/** {@inheritDoc} */
diff --git a/commons-geometry-euclidean/src/main/java/org/apache/commons/geometry/euclidean/threed/Rotation.java b/commons-geometry-euclidean/src/main/java/org/apache/commons/geometry/euclidean/threed/Rotation.java
deleted file mode 100644
index b845759..0000000
--- a/commons-geometry-euclidean/src/main/java/org/apache/commons/geometry/euclidean/threed/Rotation.java
+++ /dev/null
@@ -1,1448 +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.geometry.euclidean.threed;
-
-import java.io.Serializable;
-
-import org.apache.commons.geometry.core.exception.GeometryException;
-import org.apache.commons.geometry.core.exception.IllegalNormException;
-import org.apache.commons.geometry.euclidean.internal.Vectors;
-import org.apache.commons.numbers.arrays.LinearCombination;
-
-/**
- * This class implements rotations in a three-dimensional space.
- *
- * <p>Rotations can be represented by several different mathematical
- * entities (matrices, axe and angle, Cardan or Euler angles,
- * quaternions). This class presents an higher level abstraction, more
- * user-oriented and hiding this implementation details. Well, for the
- * curious, we use quaternions for the internal representation. The
- * user can build a rotation from any of these representations, and
- * any of these representations can be retrieved from a
- * <code>Rotation</code> instance (see the various constructors and
- * getters). In addition, a rotation can also be built implicitly
- * from a set of vectors and their image.</p>
- * <p>This implies that this class can be used to convert from one
- * representation to another one. For example, converting a rotation
- * matrix into a set of Cardan angles from can be done using the
- * following single line of code:</p>
- * <pre>
- * double[] angles = new Rotation(matrix, 1.0e-10).getAngles(RotationOrder.XYZ);
- * </pre>
- * <p>Focus is oriented on what a rotation <em>do</em> rather than on its
- * underlying representation. Once it has been built, and regardless of its
- * internal representation, a rotation is an <em>operator</em> which basically
- * transforms three dimensional {@link Vector3D vectors} into other three
- * dimensional {@link Vector3D vectors}. Depending on the application, the
- * meaning of these vectors may vary and the semantics of the rotation also.</p>
- * <p>For example in an spacecraft attitude simulation tool, users will often
- * consider the vectors are fixed (say the Earth direction for example) and the
- * frames change. The rotation transforms the coordinates of the vector in inertial
- * frame into the coordinates of the same vector in satellite frame. In this
- * case, the rotation implicitly defines the relation between the two frames.</p>
- * <p>Another example could be a telescope control application, where the rotation
- * would transform the sighting direction at rest into the desired observing
- * direction when the telescope is pointed towards an object of interest. In this
- * case the rotation transforms the direction at rest in a topocentric frame
- * into the sighting direction in the same topocentric frame. This implies in this
- * case the frame is fixed and the vector moves.</p>
- * <p>In many case, both approaches will be combined. In our telescope example,
- * we will probably also need to transform the observing direction in the topocentric
- * frame into the observing direction in inertial frame taking into account the observatory
- * location and the Earth rotation, which would essentially be an application of the
- * first approach.</p>
- *
- * <p>These examples show that a rotation is what the user wants it to be. This
- * class does not push the user towards one specific definition and hence does not
- * provide methods like <code>projectVectorIntoDestinationFrame</code> or
- * <code>computeTransformedDirection</code>. It provides simpler and more generic
- * methods: {@link #applyTo(Vector3D) applyTo(Vector3D)} and {@link
- * #applyInverseTo(Vector3D) applyInverseTo(Vector3D)}.</p>
- *
- * <p>Since a rotation is basically a vectorial operator, several rotations can be
- * composed together and the composite operation <code>r = r<sub>1</sub> o
- * r<sub>2</sub></code> (which means that for each vector <code>u</code>,
- * <code>r(u) = r<sub>1</sub>(r<sub>2</sub>(u))</code>) is also a rotation. Hence
- * we can consider that in addition to vectors, a rotation can be applied to other
- * rotations as well (or to itself). With our previous notations, we would say we
- * can apply <code>r<sub>1</sub></code> to <code>r<sub>2</sub></code> and the result
- * we get is <code>r = r<sub>1</sub> o r<sub>2</sub></code>. For this purpose, the
- * class provides the methods: {@link #applyTo(Rotation) applyTo(Rotation)} and
- * {@link #applyInverseTo(Rotation) applyInverseTo(Rotation)}.</p>
- *
- * <p>Rotations are guaranteed to be immutable objects.</p>
- *
- * @see Vector3D
- * @see RotationOrder
- */
-
-public class Rotation implements Serializable {
-
- /** Identity rotation. */
- public static final Rotation IDENTITY = new Rotation(1.0, 0.0, 0.0, 0.0, false);
-
- /** Serializable version identifier */
- private static final long serialVersionUID = 20180903L;
-
- /** Scalar coordinate of the quaternion. */
- private final double q0;
-
- /** First coordinate of the vectorial part of the quaternion. */
- private final double q1;
-
- /** Second coordinate of the vectorial part of the quaternion. */
- private final double q2;
-
- /** Third coordinate of the vectorial part of the quaternion. */
- private final double q3;
-
- /** Build a rotation from the quaternion coordinates.
- * <p>A rotation can be built from a <em>normalized</em> quaternion,
- * i.e. a quaternion for which q<sub>0</sub><sup>2</sup> +
- * q<sub>1</sub><sup>2</sup> + q<sub>2</sub><sup>2</sup> +
- * q<sub>3</sub><sup>2</sup> = 1. If the quaternion is not normalized,
- * the constructor can normalize it in a preprocessing step.</p>
- * <p>Note that some conventions put the scalar part of the quaternion
- * as the 4<sup>th</sup> component and the vector part as the first three
- * components. This is <em>not</em> our convention. We put the scalar part
- * as the first component.</p>
- * @param q0 scalar part of the quaternion
- * @param q1 first coordinate of the vectorial part of the quaternion
- * @param q2 second coordinate of the vectorial part of the quaternion
- * @param q3 third coordinate of the vectorial part of the quaternion
- * @param needsNormalization if true, the coordinates are considered
- * not to be normalized, a normalization preprocessing step is performed
- * before using them
- */
- public Rotation(double q0, double q1, double q2, double q3,
- boolean needsNormalization) {
-
- if (needsNormalization) {
- // normalization preprocessing
- double inv = 1.0 / Math.sqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
- q0 *= inv;
- q1 *= inv;
- q2 *= inv;
- q3 *= inv;
- }
-
- this.q0 = q0;
- this.q1 = q1;
- this.q2 = q2;
- this.q3 = q3;
-
- }
-
- /** Build a rotation from an axis and an angle.
- * <p>
- * Calling this constructor is equivalent to call
- * {@link #Rotation(Vector3D, double, RotationConvention)
- * new Rotation(axis, angle, RotationConvention.VECTOR_OPERATOR)}
- * </p>
- * @param axis axis around which to rotate
- * @param angle rotation angle.
- * @exception IllegalArgumentException if the axis norm is zero
- * @deprecated as of 3.6, replaced with {@link #Rotation(Vector3D, double, RotationConvention)}
- */
- @Deprecated
- public Rotation(Vector3D axis, double angle) {
- this(axis, angle, RotationConvention.VECTOR_OPERATOR);
- }
-
- /** Build a rotation from an axis and an angle.
- * @param axis axis around which to rotate
- * @param angle rotation angle
- * @param convention convention to use for the semantics of the angle
- * @exception IllegalNormException if the axis norm is zero, NaN, or infinite
- */
- public Rotation(final Vector3D axis, final double angle, final RotationConvention convention)
- throws IllegalNormException {
-
- double norm = Vectors.checkedNorm(axis);
-
- double halfAngle = convention == RotationConvention.VECTOR_OPERATOR ? -0.5 * angle : +0.5 * angle;
- double coeff = Math.sin(halfAngle) / norm;
-
- q0 = Math.cos (halfAngle);
- q1 = coeff * axis.getX();
- q2 = coeff * axis.getY();
- q3 = coeff * axis.getZ();
-
- }
-
- /** Build a rotation from a 3X3 matrix.
-
- * <p>Rotation matrices are orthogonal matrices, i.e. unit matrices
- * (which are matrices for which m.m<sup>T</sup> = I) with real
- * coefficients. The module of the determinant of unit matrices is
- * 1, among the orthogonal 3X3 matrices, only the ones having a
- * positive determinant (+1) are rotation matrices.</p>
-
- * <p>When a rotation is defined by a matrix with truncated values
- * (typically when it is extracted from a technical sheet where only
- * four to five significant digits are available), the matrix is not
- * orthogonal anymore. This constructor handles this case
- * transparently by using a copy of the given matrix and applying a
- * correction to the copy in order to perfect its orthogonality. If
- * the Frobenius norm of the correction needed is above the given
- * threshold, then the matrix is considered to be too far from a
- * true rotation matrix and an exception is thrown.<p>
-
- * @param m rotation matrix
- * @param threshold convergence threshold for the iterative
- * orthogonality correction (convergence is reached when the
- * difference between two steps of the Frobenius norm of the
- * correction is below this threshold)
-
- * @exception IllegalArgumentException if the matrix is not a 3X3
- * matrix, or if it cannot be transformed into an orthogonal matrix
- * with the given threshold, or if the determinant of the resulting
- * orthogonal matrix is negative
- */
- public Rotation(double[][] m, double threshold)
- throws IllegalArgumentException {
-
- // dimension check
- if ((m.length != 3) || (m[0].length != 3) ||
- (m[1].length != 3) || (m[2].length != 3)) {
- throw new IllegalArgumentException("A " + m.length + "x" + m[0].length + " matrix cannot be a rotation matrix");
- }
-
- // compute a "close" orthogonal matrix
- double[][] ort = orthogonalizeMatrix(m, threshold);
-
- // check the sign of the determinant
- double det = ort[0][0] * (ort[1][1] * ort[2][2] - ort[2][1] * ort[1][2]) -
- ort[1][0] * (ort[0][1] * ort[2][2] - ort[2][1] * ort[0][2]) +
- ort[2][0] * (ort[0][1] * ort[1][2] - ort[1][1] * ort[0][2]);
- if (det < 0.0) {
- throw new IllegalArgumentException("The closest orthogonal matrix has a negative determinant " + det);
- }
-
- double[] quat = mat2quat(ort);
- q0 = quat[0];
- q1 = quat[1];
- q2 = quat[2];
- q3 = quat[3];
-
- }
-
- /** Build the rotation that transforms a pair of vectors into another pair.
-
- * <p>Except for possible scale factors, if the instance were applied to
- * the pair (u<sub>1</sub>, u<sub>2</sub>) it will produce the pair
- * (v<sub>1</sub>, v<sub>2</sub>).</p>
-
- * <p>If the angular separation between u<sub>1</sub> and u<sub>2</sub> is
- * not the same as the angular separation between v<sub>1</sub> and
- * v<sub>2</sub>, then a corrected v'<sub>2</sub> will be used rather than
- * v<sub>2</sub>, the corrected vector will be in the (±v<sub>1</sub>,
- * +v<sub>2</sub>) half-plane.</p>
-
- * @param u1 first vector of the origin pair
- * @param u2 second vector of the origin pair
- * @param v1 desired image of u1 by the rotation
- * @param v2 desired image of u2 by the rotation
- * @exception IllegalNormException if the norm of one of the vectors is zero, NaN, infinite,
- * or if one of the pair is degenerated (i.e. the vectors of the pair are collinear)
- */
- public Rotation(Vector3D u1, Vector3D u2, Vector3D v1, Vector3D v2)
- throws IllegalNormException {
-
- // build orthonormalized base from u1, u2
- // this fails when vectors are null or collinear, which is forbidden to define a rotation
- final Vector3D u3 = u1.crossProduct(u2).normalize();
- u2 = u3.crossProduct(u1).normalize();
- u1 = u1.normalize();
-
- // build an orthonormalized base from v1, v2
- // this fails when vectors are null or collinear, which is forbidden to define a rotation
- final Vector3D v3 = v1.crossProduct(v2).normalize();
- v2 = v3.crossProduct(v1).normalize();
- v1 = v1.normalize();
-
- // buid a matrix transforming the first base into the second one
- final double[][] m = new double[][] {
- {
- LinearCombination.value(u1.getX(), v1.getX(), u2.getX(), v2.getX(), u3.getX(), v3.getX()),
- LinearCombination.value(u1.getY(), v1.getX(), u2.getY(), v2.getX(), u3.getY(), v3.getX()),
- LinearCombination.value(u1.getZ(), v1.getX(), u2.getZ(), v2.getX(), u3.getZ(), v3.getX())
- },
- {
- LinearCombination.value(u1.getX(), v1.getY(), u2.getX(), v2.getY(), u3.getX(), v3.getY()),
- LinearCombination.value(u1.getY(), v1.getY(), u2.getY(), v2.getY(), u3.getY(), v3.getY()),
- LinearCombination.value(u1.getZ(), v1.getY(), u2.getZ(), v2.getY(), u3.getZ(), v3.getY())
- },
- {
- LinearCombination.value(u1.getX(), v1.getZ(), u2.getX(), v2.getZ(), u3.getX(), v3.getZ()),
- LinearCombination.value(u1.getY(), v1.getZ(), u2.getY(), v2.getZ(), u3.getY(), v3.getZ()),
- LinearCombination.value(u1.getZ(), v1.getZ(), u2.getZ(), v2.getZ(), u3.getZ(), v3.getZ())
- }
- };
-
- double[] quat = mat2quat(m);
- q0 = quat[0];
- q1 = quat[1];
- q2 = quat[2];
- q3 = quat[3];
- }
-
- /** Build one of the rotations that transform one vector into another one.
-
- * <p>Except for a possible scale factor, if the instance were
- * applied to the vector u it will produce the vector v. There is an
- * infinite number of such rotations, this constructor choose the
- * one with the smallest associated angle (i.e. the one whose axis
- * is orthogonal to the (u, v) plane). If u and v are collinear, an
- * arbitrary rotation axis is chosen.</p>
-
- * @param u origin vector
- * @param v desired image of u by the rotation
- * @exception IllegalNormException if the norm of one of the vectors is zero, NaN, or infinite
- */
- public Rotation(Vector3D u, Vector3D v) {
-
- double normProduct = Vectors.checkedNorm(u) * Vectors.checkedNorm(v);
-
- double dot = u.dotProduct(v);
-
- if (dot < ((2.0e-15 - 1.0) * normProduct)) {
- // special case u = -v: we select a PI angle rotation around
- // an arbitrary vector orthogonal to u
- Vector3D w = u.orthogonal();
- q0 = 0.0;
- q1 = -w.getX();
- q2 = -w.getY();
- q3 = -w.getZ();
- } else {
- // general case: (u, v) defines a plane, we select
- // the shortest possible rotation: axis orthogonal to this plane
- q0 = Math.sqrt(0.5 * (1.0 + dot / normProduct));
- double coeff = 1.0 / (2.0 * q0 * normProduct);
- Vector3D q = v.crossProduct(u);
- q1 = coeff * q.getX();
- q2 = coeff * q.getY();
- q3 = coeff * q.getZ();
- }
-
- }
-
- /** Build a rotation from three Cardan or Euler elementary rotations.
-
- * <p>
- * Calling this constructor is equivalent to call
- * {@link #Rotation(RotationOrder, RotationConvention, double, double, double)
- * new Rotation(order, RotationConvention.VECTOR_OPERATOR, alpha1, alpha2, alpha3)}
- * </p>
-
- * @param order order of rotations to use
- * @param alpha1 angle of the first elementary rotation
- * @param alpha2 angle of the second elementary rotation
- * @param alpha3 angle of the third elementary rotation
- * @deprecated as of 3.6, replaced with {@link
- * #Rotation(RotationOrder, RotationConvention, double, double, double)}
- */
- @Deprecated
- public Rotation(RotationOrder order,
- double alpha1, double alpha2, double alpha3) {
- this(order, RotationConvention.VECTOR_OPERATOR, alpha1, alpha2, alpha3);
- }
-
- /** Build a rotation from three Cardan or Euler elementary rotations.
-
- * <p>Cardan rotations are three successive rotations around the
- * canonical axes X, Y and Z, each axis being used once. There are
- * 6 such sets of rotations (XYZ, XZY, YXZ, YZX, ZXY and ZYX). Euler
- * rotations are three successive rotations around the canonical
- * axes X, Y and Z, the first and last rotations being around the
- * same axis. There are 6 such sets of rotations (XYX, XZX, YXY,
- * YZY, ZXZ and ZYZ), the most popular one being ZXZ.</p>
- * <p>Beware that many people routinely use the term Euler angles even
- * for what really are Cardan angles (this confusion is especially
- * widespread in the aerospace business where Roll, Pitch and Yaw angles
- * are often wrongly tagged as Euler angles).</p>
-
- * @param order order of rotations to compose, from left to right
- * (i.e. we will use {@code r1.compose(r2.compose(r3, convention), convention)})
- * @param convention convention to use for the semantics of the angle
- * @param alpha1 angle of the first elementary rotation
- * @param alpha2 angle of the second elementary rotation
- * @param alpha3 angle of the third elementary rotation
- */
- public Rotation(RotationOrder order, RotationConvention convention,
- double alpha1, double alpha2, double alpha3) {
- Rotation r1 = new Rotation(order.getA1(), alpha1, convention);
- Rotation r2 = new Rotation(order.getA2(), alpha2, convention);
- Rotation r3 = new Rotation(order.getA3(), alpha3, convention);
- Rotation composed = r1.compose(r2.compose(r3, convention), convention);
- q0 = composed.q0;
- q1 = composed.q1;
- q2 = composed.q2;
- q3 = composed.q3;
- }
-
- /** Convert an orthogonal rotation matrix to a quaternion.
- * @param ort orthogonal rotation matrix
- * @return quaternion corresponding to the matrix
- */
- private static double[] mat2quat(final double[][] ort) {
-
- final double[] quat = new double[4];
-
- // There are different ways to compute the quaternions elements
- // from the matrix. They all involve computing one element from
- // the diagonal of the matrix, and computing the three other ones
- // using a formula involving a division by the first element,
- // which unfortunately can be zero. Since the norm of the
- // quaternion is 1, we know at least one element has an absolute
- // value greater or equal to 0.5, so it is always possible to
- // select the right formula and avoid division by zero and even
- // numerical inaccuracy. Checking the elements in turn and using
- // the first one greater than 0.45 is safe (this leads to a simple
- // test since qi = 0.45 implies 4 qi^2 - 1 = -0.19)
- double s = ort[0][0] + ort[1][1] + ort[2][2];
- if (s > -0.19) {
- // compute q0 and deduce q1, q2 and q3
- quat[0] = 0.5 * Math.sqrt(s + 1.0);
- double inv = 0.25 / quat[0];
- quat[1] = inv * (ort[1][2] - ort[2][1]);
- quat[2] = inv * (ort[2][0] - ort[0][2]);
- quat[3] = inv * (ort[0][1] - ort[1][0]);
- } else {
- s = ort[0][0] - ort[1][1] - ort[2][2];
- if (s > -0.19) {
- // compute q1 and deduce q0, q2 and q3
- quat[1] = 0.5 * Math.sqrt(s + 1.0);
- double inv = 0.25 / quat[1];
- quat[0] = inv * (ort[1][2] - ort[2][1]);
- quat[2] = inv * (ort[0][1] + ort[1][0]);
- quat[3] = inv * (ort[0][2] + ort[2][0]);
- } else {
- s = ort[1][1] - ort[0][0] - ort[2][2];
- if (s > -0.19) {
- // compute q2 and deduce q0, q1 and q3
- quat[2] = 0.5 * Math.sqrt(s + 1.0);
- double inv = 0.25 / quat[2];
- quat[0] = inv * (ort[2][0] - ort[0][2]);
- quat[1] = inv * (ort[0][1] + ort[1][0]);
- quat[3] = inv * (ort[2][1] + ort[1][2]);
- } else {
- // compute q3 and deduce q0, q1 and q2
- s = ort[2][2] - ort[0][0] - ort[1][1];
- quat[3] = 0.5 * Math.sqrt(s + 1.0);
- double inv = 0.25 / quat[3];
- quat[0] = inv * (ort[0][1] - ort[1][0]);
- quat[1] = inv * (ort[0][2] + ort[2][0]);
- quat[2] = inv * (ort[2][1] + ort[1][2]);
- }
- }
- }
-
- return quat;
-
- }
-
- /** Revert a rotation.
- * Build a rotation which reverse the effect of another
- * rotation. This means that if r(u) = v, then r.revert(v) = u. The
- * instance is not changed.
- * @return a new rotation whose effect is the reverse of the effect
- * of the instance
- */
- public Rotation revert() {
- return new Rotation(-q0, q1, q2, q3, false);
- }
-
- /** Get the scalar coordinate of the quaternion.
- * @return scalar coordinate of the quaternion
- */
- public double getQ0() {
- return q0;
- }
-
- /** Get the first coordinate of the vectorial part of the quaternion.
- * @return first coordinate of the vectorial part of the quaternion
- */
- public double getQ1() {
- return q1;
- }
-
- /** Get the second coordinate of the vectorial part of the quaternion.
- * @return second coordinate of the vectorial part of the quaternion
- */
- public double getQ2() {
- return q2;
- }
-
- /** Get the third coordinate of the vectorial part of the quaternion.
- * @return third coordinate of the vectorial part of the quaternion
- */
- public double getQ3() {
- return q3;
- }
-
- /** Get the normalized axis of the rotation.
- * <p>
- * Calling this method is equivalent to call
- * {@link #getAxis(RotationConvention) getAxis(RotationConvention.VECTOR_OPERATOR)}
- * </p>
- * @return normalized axis of the rotation
- * @see #Rotation(Vector3D, double, RotationConvention)
- * @deprecated as of 3.6, replaced with {@link #getAxis(RotationConvention)}
- */
- @Deprecated
- public Vector3D getAxis() {
- return getAxis(RotationConvention.VECTOR_OPERATOR);
- }
-
- /** Get the normalized axis of the rotation.
- * <p>
- * Note that as {@link #getAngle()} always returns an angle
- * between 0 and π, changing the convention changes the
- * direction of the axis, not the sign of the angle.
- * </p>
- * @param convention convention to use for the semantics of the angle
- * @return normalized axis of the rotation
- * @see #Rotation(Vector3D, double, RotationConvention)
- */
- public Vector3D getAxis(final RotationConvention convention) {
- final double squaredSine = q1 * q1 + q2 * q2 + q3 * q3;
- if (squaredSine == 0) {
- return convention == RotationConvention.VECTOR_OPERATOR ? Vector3D.PLUS_X : Vector3D.MINUS_X;
- } else {
- final double sgn = convention == RotationConvention.VECTOR_OPERATOR ? +1 : -1;
- if (q0 < 0) {
- final double inverse = sgn / Math.sqrt(squaredSine);
- return Vector3D.of(q1 * inverse, q2 * inverse, q3 * inverse);
- }
- final double inverse = -sgn / Math.sqrt(squaredSine);
- return Vector3D.of(q1 * inverse, q2 * inverse, q3 * inverse);
- }
- }
-
- /** Get the angle of the rotation.
- * @return angle of the rotation (between 0 and π)
- * @see #Rotation(Vector3D, double)
- */
- public double getAngle() {
- if ((q0 < -0.1) || (q0 > 0.1)) {
- return 2 * Math.asin(Math.sqrt(q1 * q1 + q2 * q2 + q3 * q3));
- } else if (q0 < 0) {
- return 2 * Math.acos(-q0);
- }
- return 2 * Math.acos(q0);
- }
-
- /** Get the Cardan or Euler angles corresponding to the instance.
-
- * <p>
- * Calling this method is equivalent to call
- * {@link #getAngles(RotationOrder, RotationConvention)
- * getAngles(order, RotationConvention.VECTOR_OPERATOR)}
- * </p>
-
- * @param order rotation order to use
- * @return an array of three angles, in the order specified by the set
- * @exception AngleSetSingularityException if the rotation is
- * singular with respect to the angles set specified
- * @deprecated as of 3.6, replaced with {@link #getAngles(RotationOrder, RotationConvention)}
- */
- @Deprecated
- public double[] getAngles(RotationOrder order)
- throws AngleSetSingularityException {
- return getAngles(order, RotationConvention.VECTOR_OPERATOR);
- }
-
- /** Get the Cardan or Euler angles corresponding to the instance.
-
- * <p>The equations show that each rotation can be defined by two
- * different values of the Cardan or Euler angles set. For example
- * if Cardan angles are used, the rotation defined by the angles
- * a<sub>1</sub>, a<sub>2</sub> and a<sub>3</sub> is the same as
- * the rotation defined by the angles π + a<sub>1</sub>, π
- * - a<sub>2</sub> and π + a<sub>3</sub>. This method implements
- * the following arbitrary choices:</p>
- * <ul>
- * <li>for Cardan angles, the chosen set is the one for which the
- * second angle is between -π/2 and π/2 (i.e its cosine is
- * positive),</li>
- * <li>for Euler angles, the chosen set is the one for which the
- * second angle is between 0 and π (i.e its sine is positive).</li>
- * </ul>
-
- * <p>Cardan and Euler angle have a very disappointing drawback: all
- * of them have singularities. This means that if the instance is
- * too close to the singularities corresponding to the given
- * rotation order, it will be impossible to retrieve the angles. For
- * Cardan angles, this is often called gimbal lock. There is
- * <em>nothing</em> to do to prevent this, it is an intrinsic problem
- * with Cardan and Euler representation (but not a problem with the
- * rotation itself, which is perfectly well defined). For Cardan
- * angles, singularities occur when the second angle is close to
- * -π/2 or +π/2, for Euler angle singularities occur when the
- * second angle is close to 0 or π, this implies that the identity
- * rotation is always singular for Euler angles!</p>
-
- * @param order rotation order to use
- * @param convention convention to use for the semantics of the angle
- * @return an array of three angles, in the order specified by the set
- * @exception AngleSetSingularityException if the rotation is
- * singular with respect to the angle set specified
- */
- public double[] getAngles(RotationOrder order, RotationConvention convention)
- throws AngleSetSingularityException {
-
- if (convention == RotationConvention.VECTOR_OPERATOR) {
- if (order == RotationOrder.XYZ) {
-
- // r (Cartesian3D.plusK) coordinates are :
- // sin (theta), -cos (theta) sin (phi), cos (theta) cos (phi)
- // (-r) (Cartesian3D.plusI) coordinates are :
- // cos (psi) cos (theta), -sin (psi) cos (theta), sin (theta)
- // and we can choose to have theta in the interval [-PI/2 ; +PI/2]
- Vector3D v1 = applyTo(Vector3D.PLUS_Z);
- Vector3D v2 = applyInverseTo(Vector3D.PLUS_X);
- if ((v2.getZ() < -0.9999999999) || (v2.getZ() > 0.9999999999)) {
- throw new CardanSingularityException();
- }
- return new double[] {
- Math.atan2(-(v1.getY()), v1.getZ()),
- Math.asin(v2.getZ()),
- Math.atan2(-(v2.getY()), v2.getX())
- };
-
- } else if (order == RotationOrder.XZY) {
-
- // r (Cartesian3D.plusJ) coordinates are :
- // -sin (psi), cos (psi) cos (phi), cos (psi) sin (phi)
- // (-r) (Cartesian3D.plusI) coordinates are :
- // cos (theta) cos (psi), -sin (psi), sin (theta) cos (psi)
- // and we can choose to have psi in the interval [-PI/2 ; +PI/2]
- Vector3D v1 = applyTo(Vector3D.PLUS_Y);
- Vector3D v2 = applyInverseTo(Vector3D.PLUS_X);
- if ((v2.getY() < -0.9999999999) || (v2.getY() > 0.9999999999)) {
- throw new CardanSingularityException();
- }
- return new double[] {
- Math.atan2(v1.getZ(), v1.getY()),
- -Math.asin(v2.getY()),
- Math.atan2(v2.getZ(), v2.getX())
- };
-
- } else if (order == RotationOrder.YXZ) {
-
- // r (Cartesian3D.plusK) coordinates are :
- // cos (phi) sin (theta), -sin (phi), cos (phi) cos (theta)
- // (-r) (Cartesian3D.plusJ) coordinates are :
- // sin (psi) cos (phi), cos (psi) cos (phi), -sin (phi)
- // and we can choose to have phi in the interval [-PI/2 ; +PI/2]
- Vector3D v1 = applyTo(Vector3D.PLUS_Z);
- Vector3D v2 = applyInverseTo(Vector3D.PLUS_Y);
- if ((v2.getZ() < -0.9999999999) || (v2.getZ() > 0.9999999999)) {
- throw new CardanSingularityException();
- }
- return new double[] {
- Math.atan2(v1.getX(), v1.getZ()),
- -Math.asin(v2.getZ()),
- Math.atan2(v2.getX(), v2.getY())
- };
-
- } else if (order == RotationOrder.YZX) {
-
- // r (Cartesian3D.plusI) coordinates are :
- // cos (psi) cos (theta), sin (psi), -cos (psi) sin (theta)
- // (-r) (Cartesian3D.plusJ) coordinates are :
- // sin (psi), cos (phi) cos (psi), -sin (phi) cos (psi)
- // and we can choose to have psi in the interval [-PI/2 ; +PI/2]
- Vector3D v1 = applyTo(Vector3D.PLUS_X);
- Vector3D v2 = applyInverseTo(Vector3D.PLUS_Y);
- if ((v2.getX() < -0.9999999999) || (v2.getX() > 0.9999999999)) {
- throw new CardanSingularityException();
- }
- return new double[] {
- Math.atan2(-(v1.getZ()), v1.getX()),
- Math.asin(v2.getX()),
- Math.atan2(-(v2.getZ()), v2.getY())
- };
-
- } else if (order == RotationOrder.ZXY) {
-
- // r (Cartesian3D.plusJ) coordinates are :
- // -cos (phi) sin (psi), cos (phi) cos (psi), sin (phi)
- // (-r) (Cartesian3D.plusK) coordinates are :
- // -sin (theta) cos (phi), sin (phi), cos (theta) cos (phi)
- // and we can choose to have phi in the interval [-PI/2 ; +PI/2]
- Vector3D v1 = applyTo(Vector3D.PLUS_Y);
- Vector3D v2 = applyInverseTo(Vector3D.PLUS_Z);
- if ((v2.getY() < -0.9999999999) || (v2.getY() > 0.9999999999)) {
- throw new CardanSingularityException();
- }
- return new double[] {
- Math.atan2(-(v1.getX()), v1.getY()),
- Math.asin(v2.getY()),
- Math.atan2(-(v2.getX()), v2.getZ())
- };
-
- } else if (order == RotationOrder.ZYX) {
-
- // r (Cartesian3D.plusI) coordinates are :
- // cos (theta) cos (psi), cos (theta) sin (psi), -sin (theta)
- // (-r) (Cartesian3D.plusK) coordinates are :
- // -sin (theta), sin (phi) cos (theta), cos (phi) cos (theta)
- // and we can choose to have theta in the interval [-PI/2 ; +PI/2]
- Vector3D v1 = applyTo(Vector3D.PLUS_X);
- Vector3D v2 = applyInverseTo(Vector3D.PLUS_Z);
- if ((v2.getX() < -0.9999999999) || (v2.getX() > 0.9999999999)) {
- throw new CardanSingularityException();
- }
- return new double[] {
- Math.atan2(v1.getY(), v1.getX()),
- -Math.asin(v2.getX()),
- Math.atan2(v2.getY(), v2.getZ())
- };
-
- } else if (order == RotationOrder.XYX) {
-
- // r (Cartesian3D.plusI) coordinates are :
- // cos (theta), sin (phi1) sin (theta), -cos (phi1) sin (theta)
- // (-r) (Cartesian3D.plusI) coordinates are :
- // cos (theta), sin (theta) sin (phi2), sin (theta) cos (phi2)
- // and we can choose to have theta in the interval [0 ; PI]
- Vector3D v1 = applyTo(Vector3D.PLUS_X);
- Vector3D v2 = applyInverseTo(Vector3D.PLUS_X);
- if ((v2.getX() < -0.9999999999) || (v2.getX() > 0.9999999999)) {
- throw new EulerSingularityException();
- }
- return new double[] {
- Math.atan2(v1.getY(), -v1.getZ()),
- Math.acos(v2.getX()),
- Math.atan2(v2.getY(), v2.getZ())
- };
-
- } else if (order == RotationOrder.XZX) {
-
- // r (Cartesian3D.plusI) coordinates are :
- // cos (psi), cos (phi1) sin (psi), sin (phi1) sin (psi)
- // (-r) (Cartesian3D.plusI) coordinates are :
- // cos (psi), -sin (psi) cos (phi2), sin (psi) sin (phi2)
- // and we can choose to have psi in the interval [0 ; PI]
- Vector3D v1 = applyTo(Vector3D.PLUS_X);
- Vector3D v2 = applyInverseTo(Vector3D.PLUS_X);
- if ((v2.getX() < -0.9999999999) || (v2.getX() > 0.9999999999)) {
- throw new EulerSingularityException();
- }
- return new double[] {
- Math.atan2(v1.getZ(), v1.getY()),
- Math.acos(v2.getX()),
- Math.atan2(v2.getZ(), -v2.getY())
- };
-
- } else if (order == RotationOrder.YXY) {
-
- // r (Cartesian3D.plusJ) coordinates are :
- // sin (theta1) sin (phi), cos (phi), cos (theta1) sin (phi)
- // (-r) (Cartesian3D.plusJ) coordinates are :
- // sin (phi) sin (theta2), cos (phi), -sin (phi) cos (theta2)
- // and we can choose to have phi in the interval [0 ; PI]
- Vector3D v1 = applyTo(Vector3D.PLUS_Y);
- Vector3D v2 = applyInverseTo(Vector3D.PLUS_Y);
- if ((v2.getY() < -0.9999999999) || (v2.getY() > 0.9999999999)) {
- throw new EulerSingularityException();
- }
- return new double[] {
- Math.atan2(v1.getX(), v1.getZ()),
- Math.acos(v2.getY()),
- Math.atan2(v2.getX(), -v2.getZ())
- };
-
- } else if (order == RotationOrder.YZY) {
-
- // r (Cartesian3D.plusJ) coordinates are :
- // -cos (theta1) sin (psi), cos (psi), sin (theta1) sin (psi)
- // (-r) (Cartesian3D.plusJ) coordinates are :
- // sin (psi) cos (theta2), cos (psi), sin (psi) sin (theta2)
- // and we can choose to have psi in the interval [0 ; PI]
- Vector3D v1 = applyTo(Vector3D.PLUS_Y);
- Vector3D v2 = applyInverseTo(Vector3D.PLUS_Y);
- if ((v2.getY() < -0.9999999999) || (v2.getY() > 0.9999999999)) {
- throw new EulerSingularityException();
- }
- return new double[] {
- Math.atan2(v1.getZ(), -v1.getX()),
- Math.acos(v2.getY()),
- Math.atan2(v2.getZ(), v2.getX())
- };
-
- } else if (order == RotationOrder.ZXZ) {
-
- // r (Cartesian3D.plusK) coordinates are :
- // sin (psi1) sin (phi), -cos (psi1) sin (phi), cos (phi)
- // (-r) (Cartesian3D.plusK) coordinates are :
- // sin (phi) sin (psi2), sin (phi) cos (psi2), cos (phi)
- // and we can choose to have phi in the interval [0 ; PI]
- Vector3D v1 = applyTo(Vector3D.PLUS_Z);
- Vector3D v2 = applyInverseTo(Vector3D.PLUS_Z);
- if ((v2.getZ() < -0.9999999999) || (v2.getZ() > 0.9999999999)) {
- throw new EulerSingularityException();
- }
- return new double[] {
- Math.atan2(v1.getX(), -v1.getY()),
- Math.acos(v2.getZ()),
- Math.atan2(v2.getX(), v2.getY())
- };
-
- } else { // last possibility is ZYZ
-
- // r (Cartesian3D.plusK) coordinates are :
- // cos (psi1) sin (theta), sin (psi1) sin (theta), cos (theta)
- // (-r) (Cartesian3D.plusK) coordinates are :
- // -sin (theta) cos (psi2), sin (theta) sin (psi2), cos (theta)
- // and we can choose to have theta in the interval [0 ; PI]
- Vector3D v1 = applyTo(Vector3D.PLUS_Z);
- Vector3D v2 = applyInverseTo(Vector3D.PLUS_Z);
- if ((v2.getZ() < -0.9999999999) || (v2.getZ() > 0.9999999999)) {
- throw new EulerSingularityException();
- }
- return new double[] {
- Math.atan2(v1.getY(), v1.getX()),
- Math.acos(v2.getZ()),
- Math.atan2(v2.getY(), -v2.getX())
- };
-
- }
- } else {
- if (order == RotationOrder.XYZ) {
-
- // r (Cartesian3D.plusI) coordinates are :
- // cos (theta) cos (psi), -cos (theta) sin (psi), sin (theta)
- // (-r) (Cartesian3D.plusK) coordinates are :
- // sin (theta), -sin (phi) cos (theta), cos (phi) cos (theta)
- // and we can choose to have theta in the interval [-PI/2 ; +PI/2]
- Vector3D v1 = applyTo(Vector3D.PLUS_X);
- Vector3D v2 = applyInverseTo(Vector3D.PLUS_Z);
- if ((v2.getX() < -0.9999999999) || (v2.getX() > 0.9999999999)) {
- throw new CardanSingularityException();
- }
- return new double[] {
- Math.atan2(-v2.getY(), v2.getZ()),
- Math.asin(v2.getX()),
- Math.atan2(-v1.getY(), v1.getX())
- };
-
- } else if (order == RotationOrder.XZY) {
-
- // r (Cartesian3D.plusI) coordinates are :
- // cos (psi) cos (theta), -sin (psi), cos (psi) sin (theta)
- // (-r) (Cartesian3D.plusJ) coordinates are :
- // -sin (psi), cos (phi) cos (psi), sin (phi) cos (psi)
- // and we can choose to have psi in the interval [-PI/2 ; +PI/2]
- Vector3D v1 = applyTo(Vector3D.PLUS_X);
- Vector3D v2 = applyInverseTo(Vector3D.PLUS_Y);
- if ((v2.getX() < -0.9999999999) || (v2.getX() > 0.9999999999)) {
- throw new CardanSingularityException();
- }
- return new double[] {
- Math.atan2(v2.getZ(), v2.getY()),
- -Math.asin(v2.getX()),
- Math.atan2(v1.getZ(), v1.getX())
- };
-
- } else if (order == RotationOrder.YXZ) {
-
- // r (Cartesian3D.plusJ) coordinates are :
- // cos (phi) sin (psi), cos (phi) cos (psi), -sin (phi)
- // (-r) (Cartesian3D.plusK) coordinates are :
- // sin (theta) cos (phi), -sin (phi), cos (theta) cos (phi)
- // and we can choose to have phi in the interval [-PI/2 ; +PI/2]
- Vector3D v1 = applyTo(Vector3D.PLUS_Y);
- Vector3D v2 = applyInverseTo(Vector3D.PLUS_Z);
- if ((v2.getY() < -0.9999999999) || (v2.getY() > 0.9999999999)) {
- throw new CardanSingularityException();
- }
- return new double[] {
- Math.atan2(v2.getX(), v2.getZ()),
- -Math.asin(v2.getY()),
- Math.atan2(v1.getX(), v1.getY())
- };
-
- } else if (order == RotationOrder.YZX) {
-
- // r (Cartesian3D.plusJ) coordinates are :
- // sin (psi), cos (psi) cos (phi), -cos (psi) sin (phi)
- // (-r) (Cartesian3D.plusI) coordinates are :
- // cos (theta) cos (psi), sin (psi), -sin (theta) cos (psi)
- // and we can choose to have psi in the interval [-PI/2 ; +PI/2]
- Vector3D v1 = applyTo(Vector3D.PLUS_Y);
- Vector3D v2 = applyInverseTo(Vector3D.PLUS_X);
- if ((v2.getY() < -0.9999999999) || (v2.getY() > 0.9999999999)) {
- throw new CardanSingularityException();
- }
- return new double[] {
- Math.atan2(-v2.getZ(), v2.getX()),
- Math.asin(v2.getY()),
- Math.atan2(-v1.getZ(), v1.getY())
- };
-
- } else if (order == RotationOrder.ZXY) {
-
- // r (Cartesian3D.plusK) coordinates are :
- // -cos (phi) sin (theta), sin (phi), cos (phi) cos (theta)
- // (-r) (Cartesian3D.plusJ) coordinates are :
- // -sin (psi) cos (phi), cos (psi) cos (phi), sin (phi)
- // and we can choose to have phi in the interval [-PI/2 ; +PI/2]
- Vector3D v1 = applyTo(Vector3D.PLUS_Z);
- Vector3D v2 = applyInverseTo(Vector3D.PLUS_Y);
- if ((v2.getZ() < -0.9999999999) || (v2.getZ() > 0.9999999999)) {
- throw new CardanSingularityException();
- }
- return new double[] {
- Math.atan2(-v2.getX(), v2.getY()),
- Math.asin(v2.getZ()),
- Math.atan2(-v1.getX(), v1.getZ())
- };
-
- } else if (order == RotationOrder.ZYX) {
-
- // r (Cartesian3D.plusK) coordinates are :
- // -sin (theta), cos (theta) sin (phi), cos (theta) cos (phi)
- // (-r) (Cartesian3D.plusI) coordinates are :
- // cos (psi) cos (theta), sin (psi) cos (theta), -sin (theta)
- // and we can choose to have theta in the interval [-PI/2 ; +PI/2]
- Vector3D v1 = applyTo(Vector3D.PLUS_Z);
- Vector3D v2 = applyInverseTo(Vector3D.PLUS_X);
- if ((v2.getZ() < -0.9999999999) || (v2.getZ() > 0.9999999999)) {
- throw new CardanSingularityException();
- }
- return new double[] {
- Math.atan2(v2.getY(), v2.getX()),
- -Math.asin(v2.getZ()),
- Math.atan2(v1.getY(), v1.getZ())
- };
-
- } else if (order == RotationOrder.XYX) {
-
- // r (Cartesian3D.plusI) coordinates are :
- // cos (theta), sin (phi2) sin (theta), cos (phi2) sin (theta)
- // (-r) (Cartesian3D.plusI) coordinates are :
- // cos (theta), sin (theta) sin (phi1), -sin (theta) cos (phi1)
- // and we can choose to have theta in the interval [0 ; PI]
- Vector3D v1 = applyTo(Vector3D.PLUS_X);
- Vector3D v2 = applyInverseTo(Vector3D.PLUS_X);
- if ((v2.getX() < -0.9999999999) || (v2.getX() > 0.9999999999)) {
- throw new EulerSingularityException();
- }
- return new double[] {
- Math.atan2(v2.getY(), -v2.getZ()),
- Math.acos(v2.getX()),
- Math.atan2(v1.getY(), v1.getZ())
- };
-
- } else if (order == RotationOrder.XZX) {
-
- // r (Cartesian3D.plusI) coordinates are :
- // cos (psi), -cos (phi2) sin (psi), sin (phi2) sin (psi)
- // (-r) (Cartesian3D.plusI) coordinates are :
- // cos (psi), sin (psi) cos (phi1), sin (psi) sin (phi1)
- // and we can choose to have psi in the interval [0 ; PI]
- Vector3D v1 = applyTo(Vector3D.PLUS_X);
- Vector3D v2 = applyInverseTo(Vector3D.PLUS_X);
- if ((v2.getX() < -0.9999999999) || (v2.getX() > 0.9999999999)) {
- throw new EulerSingularityException();
- }
- return new double[] {
- Math.atan2(v2.getZ(), v2.getY()),
- Math.acos(v2.getX()),
- Math.atan2(v1.getZ(), -v1.getY())
- };
-
- } else if (order == RotationOrder.YXY) {
-
- // r (Cartesian3D.plusJ) coordinates are :
- // sin (phi) sin (theta2), cos (phi), -sin (phi) cos (theta2)
- // (-r) (Cartesian3D.plusJ) coordinates are :
- // sin (theta1) sin (phi), cos (phi), cos (theta1) sin (phi)
- // and we can choose to have phi in the interval [0 ; PI]
- Vector3D v1 = applyTo(Vector3D.PLUS_Y);
- Vector3D v2 = applyInverseTo(Vector3D.PLUS_Y);
- if ((v2.getY() < -0.9999999999) || (v2.getY() > 0.9999999999)) {
- throw new EulerSingularityException();
- }
- return new double[] {
- Math.atan2(v2.getX(), v2.getZ()),
- Math.acos(v2.getY()),
- Math.atan2(v1.getX(), -v1.getZ())
- };
-
- } else if (order == RotationOrder.YZY) {
-
- // r (Cartesian3D.plusJ) coordinates are :
- // sin (psi) cos (theta2), cos (psi), sin (psi) sin (theta2)
- // (-r) (Cartesian3D.plusJ) coordinates are :
- // -cos (theta1) sin (psi), cos (psi), sin (theta1) sin (psi)
- // and we can choose to have psi in the interval [0 ; PI]
- Vector3D v1 = applyTo(Vector3D.PLUS_Y);
- Vector3D v2 = applyInverseTo(Vector3D.PLUS_Y);
- if ((v2.getY() < -0.9999999999) || (v2.getY() > 0.9999999999)) {
- throw new EulerSingularityException();
- }
- return new double[] {
- Math.atan2(v2.getZ(), -v2.getX()),
- Math.acos(v2.getY()),
- Math.atan2(v1.getZ(), v1.getX())
- };
-
- } else if (order == RotationOrder.ZXZ) {
-
- // r (Cartesian3D.plusK) coordinates are :
- // sin (phi) sin (psi2), sin (phi) cos (psi2), cos (phi)
- // (-r) (Cartesian3D.plusK) coordinates are :
- // sin (psi1) sin (phi), -cos (psi1) sin (phi), cos (phi)
- // and we can choose to have phi in the interval [0 ; PI]
- Vector3D v1 = applyTo(Vector3D.PLUS_Z);
- Vector3D v2 = applyInverseTo(Vector3D.PLUS_Z);
- if ((v2.getZ() < -0.9999999999) || (v2.getZ() > 0.9999999999)) {
- throw new EulerSingularityException();
- }
- return new double[] {
- Math.atan2(v2.getX(), -v2.getY()),
- Math.acos(v2.getZ()),
- Math.atan2(v1.getX(), v1.getY())
- };
-
- } else { // last possibility is ZYZ
-
- // r (Cartesian3D.plusK) coordinates are :
- // -sin (theta) cos (psi2), sin (theta) sin (psi2), cos (theta)
- // (-r) (Cartesian3D.plusK) coordinates are :
- // cos (psi1) sin (theta), sin (psi1) sin (theta), cos (theta)
- // and we can choose to have theta in the interval [0 ; PI]
- Vector3D v1 = applyTo(Vector3D.PLUS_Z);
- Vector3D v2 = applyInverseTo(Vector3D.PLUS_Z);
- if ((v2.getZ() < -0.9999999999) || (v2.getZ() > 0.9999999999)) {
- throw new EulerSingularityException();
- }
- return new double[] {
- Math.atan2(v2.getY(), v2.getX()),
- Math.acos(v2.getZ()),
- Math.atan2(v1.getY(), -v1.getX())
- };
-
- }
- }
-
- }
-
- /** Get the 3X3 matrix corresponding to the instance
- * @return the matrix corresponding to the instance
- */
- public double[][] getMatrix() {
-
- // products
- double q0q0 = q0 * q0;
- double q0q1 = q0 * q1;
- double q0q2 = q0 * q2;
- double q0q3 = q0 * q3;
- double q1q1 = q1 * q1;
- double q1q2 = q1 * q2;
- double q1q3 = q1 * q3;
- double q2q2 = q2 * q2;
- double q2q3 = q2 * q3;
- double q3q3 = q3 * q3;
-
- // create the matrix
- double[][] m = new double[3][];
- m[0] = new double[3];
- m[1] = new double[3];
- m[2] = new double[3];
-
- m [0][0] = 2.0 * (q0q0 + q1q1) - 1.0;
- m [1][0] = 2.0 * (q1q2 - q0q3);
- m [2][0] = 2.0 * (q1q3 + q0q2);
-
- m [0][1] = 2.0 * (q1q2 + q0q3);
- m [1][1] = 2.0 * (q0q0 + q2q2) - 1.0;
- m [2][1] = 2.0 * (q2q3 - q0q1);
-
- m [0][2] = 2.0 * (q1q3 - q0q2);
- m [1][2] = 2.0 * (q2q3 + q0q1);
- m [2][2] = 2.0 * (q0q0 + q3q3) - 1.0;
-
- return m;
-
- }
-
- /** Apply the rotation to a vector.
- * @param u vector to apply the rotation to
- * @return a new vector which is the image of u by the rotation
- */
- public Vector3D applyTo(Vector3D u) {
-
- double x = u.getX();
- double y = u.getY();
- double z = u.getZ();
-
- double s = q1 * x + q2 * y + q3 * z;
-
- return Vector3D.of(2 * (q0 * (x * q0 - (q2 * z - q3 * y)) + s * q1) - x,
- 2 * (q0 * (y * q0 - (q3 * x - q1 * z)) + s * q2) - y,
- 2 * (q0 * (z * q0 - (q1 * y - q2 * x)) + s * q3) - z);
-
- }
-
- /** Apply the rotation to a vector stored in an array.
- * @param in an array with three items which stores vector to rotate
- * @param out an array with three items to put result to (it can be the same
- * array as in)
- */
- public void applyTo(final double[] in, final double[] out) {
-
- final double x = in[0];
- final double y = in[1];
- final double z = in[2];
-
- final double s = q1 * x + q2 * y + q3 * z;
-
- out[0] = 2 * (q0 * (x * q0 - (q2 * z - q3 * y)) + s * q1) - x;
- out[1] = 2 * (q0 * (y * q0 - (q3 * x - q1 * z)) + s * q2) - y;
- out[2] = 2 * (q0 * (z * q0 - (q1 * y - q2 * x)) + s * q3) - z;
-
- }
-
- /** Apply the inverse of the rotation to a vector.
- * @param u vector to apply the inverse of the rotation to
- * @return a new vector which such that u is its image by the rotation
- */
- public Vector3D applyInverseTo(Vector3D u) {
-
- double x = u.getX();
- double y = u.getY();
- double z = u.getZ();
-
- double s = q1 * x + q2 * y + q3 * z;
- double m0 = -q0;
-
- return Vector3D.of(2 * (m0 * (x * m0 - (q2 * z - q3 * y)) + s * q1) - x,
- 2 * (m0 * (y * m0 - (q3 * x - q1 * z)) + s * q2) - y,
- 2 * (m0 * (z * m0 - (q1 * y - q2 * x)) + s * q3) - z);
-
- }
-
- /** Apply the inverse of the rotation to a vector stored in an array.
- * @param in an array with three items which stores vector to rotate
- * @param out an array with three items to put result to (it can be the same
- * array as in)
- */
- public void applyInverseTo(final double[] in, final double[] out) {
-
- final double x = in[0];
- final double y = in[1];
- final double z = in[2];
-
- final double s = q1 * x + q2 * y + q3 * z;
- final double m0 = -q0;
-
- out[0] = 2 * (m0 * (x * m0 - (q2 * z - q3 * y)) + s * q1) - x;
- out[1] = 2 * (m0 * (y * m0 - (q3 * x - q1 * z)) + s * q2) - y;
- out[2] = 2 * (m0 * (z * m0 - (q1 * y - q2 * x)) + s * q3) - z;
-
- }
-
- /** Apply the instance to another rotation.
- * <p>
- * Calling this method is equivalent to call
- * {@link #compose(Rotation, RotationConvention)
- * compose(r, RotationConvention.VECTOR_OPERATOR)}.
- * </p>
- * @param r rotation to apply the rotation to
- * @return a new rotation which is the composition of r by the instance
- */
- public Rotation applyTo(Rotation r) {
- return compose(r, RotationConvention.VECTOR_OPERATOR);
- }
-
- /** Compose the instance with another rotation.
- * <p>
- * If the semantics of the rotations composition corresponds to a
- * {@link RotationConvention#VECTOR_OPERATOR vector operator} convention,
- * applying the instance to a rotation is computing the composition
- * in an order compliant with the following rule : let {@code u} be any
- * vector and {@code v} its image by {@code r1} (i.e.
- * {@code r1.applyTo(u) = v}). Let {@code w} be the image of {@code v} by
- * rotation {@code r2} (i.e. {@code r2.applyTo(v) = w}). Then
- * {@code w = comp.applyTo(u)}, where
- * {@code comp = r2.compose(r1, RotationConvention.VECTOR_OPERATOR)}.
- * </p>
- * <p>
- * If the semantics of the rotations composition corresponds to a
- * {@link RotationConvention#FRAME_TRANSFORM frame transform} convention,
- * the application order will be reversed. So keeping the exact same
- * meaning of all {@code r1}, {@code r2}, {@code u}, {@code v}, {@code w}
- * and {@code comp} as above, {@code comp} could also be computed as
- * {@code comp = r1.compose(r2, RotationConvention.FRAME_TRANSFORM)}.
- * </p>
- * @param r rotation to apply the rotation to
- * @param convention convention to use for the semantics of the angle
- * @return a new rotation which is the composition of r by the instance
- */
- public Rotation compose(final Rotation r, final RotationConvention convention) {
- return convention == RotationConvention.VECTOR_OPERATOR ?
- composeInternal(r) : r.composeInternal(this);
- }
-
- /** Compose the instance with another rotation using vector operator convention.
- * @param r rotation to apply the rotation to
- * @return a new rotation which is the composition of r by the instance
- * using vector operator convention
- */
- private Rotation composeInternal(final Rotation r) {
- return new Rotation(r.q0 * q0 - (r.q1 * q1 + r.q2 * q2 + r.q3 * q3),
- r.q1 * q0 + r.q0 * q1 + (r.q2 * q3 - r.q3 * q2),
- r.q2 * q0 + r.q0 * q2 + (r.q3 * q1 - r.q1 * q3),
- r.q3 * q0 + r.q0 * q3 + (r.q1 * q2 - r.q2 * q1),
- false);
- }
-
- /** Apply the inverse of the instance to another rotation.
- * <p>
- * Calling this method is equivalent to call
- * {@link #composeInverse(Rotation, RotationConvention)
- * composeInverse(r, RotationConvention.VECTOR_OPERATOR)}.
- * </p>
- * @param r rotation to apply the rotation to
- * @return a new rotation which is the composition of r by the inverse
- * of the instance
- */
- public Rotation applyInverseTo(Rotation r) {
- return composeInverse(r, RotationConvention.VECTOR_OPERATOR);
- }
-
- /** Compose the inverse of the instance with another rotation.
- * <p>
- * If the semantics of the rotations composition corresponds to a
- * {@link RotationConvention#VECTOR_OPERATOR vector operator} convention,
- * applying the inverse of the instance to a rotation is computing
- * the composition in an order compliant with the following rule :
- * let {@code u} be any vector and {@code v} its image by {@code r1}
- * (i.e. {@code r1.applyTo(u) = v}). Let {@code w} be the inverse image
- * of {@code v} by {@code r2} (i.e. {@code r2.applyInverseTo(v) = w}).
- * Then {@code w = comp.applyTo(u)}, where
- * {@code comp = r2.composeInverse(r1)}.
- * </p>
- * <p>
- * If the semantics of the rotations composition corresponds to a
- * {@link RotationConvention#FRAME_TRANSFORM frame transform} convention,
- * the application order will be reversed, which means it is the
- * <em>innermost</em> rotation that will be reversed. So keeping the exact same
- * meaning of all {@code r1}, {@code r2}, {@code u}, {@code v}, {@code w}
- * and {@code comp} as above, {@code comp} could also be computed as
- * {@code comp = r1.revert().composeInverse(r2.revert(), RotationConvention.FRAME_TRANSFORM)}.
- * </p>
- * @param r rotation to apply the rotation to
- * @param convention convention to use for the semantics of the angle
- * @return a new rotation which is the composition of r by the inverse
- * of the instance
- */
- public Rotation composeInverse(final Rotation r, final RotationConvention convention) {
- return convention == RotationConvention.VECTOR_OPERATOR ?
- composeInverseInternal(r) : r.composeInternal(revert());
- }
-
- /** Compose the inverse of the instance with another rotation
- * using vector operator convention.
- * @param r rotation to apply the rotation to
- * @return a new rotation which is the composition of r by the inverse
- * of the instance using vector operator convention
- */
- private Rotation composeInverseInternal(Rotation r) {
- return new Rotation(-r.q0 * q0 - (r.q1 * q1 + r.q2 * q2 + r.q3 * q3),
- -r.q1 * q0 + r.q0 * q1 + (r.q2 * q3 - r.q3 * q2),
- -r.q2 * q0 + r.q0 * q2 + (r.q3 * q1 - r.q1 * q3),
- -r.q3 * q0 + r.q0 * q3 + (r.q1 * q2 - r.q2 * q1),
- false);
- }
-
- /** Perfect orthogonality on a 3X3 matrix.
- * @param m initial matrix (not exactly orthogonal)
- * @param threshold convergence threshold for the iterative
- * orthogonality correction (convergence is reached when the
- * difference between two steps of the Frobenius norm of the
- * correction is below this threshold)
- * @return an orthogonal matrix close to m
- * @exception IllegalArgumentException if the matrix cannot be
- * orthogonalized with the given threshold after 10 iterations
- */
- private double[][] orthogonalizeMatrix(double[][] m, double threshold)
- throws IllegalArgumentException {
- double[] m0 = m[0];
- double[] m1 = m[1];
- double[] m2 = m[2];
- double x00 = m0[0];
- double x01 = m0[1];
- double x02 = m0[2];
- double x10 = m1[0];
- double x11 = m1[1];
- double x12 = m1[2];
- double x20 = m2[0];
- double x21 = m2[1];
- double x22 = m2[2];
- double fn = 0;
- double fn1;
-
- double[][] o = new double[3][3];
- double[] o0 = o[0];
- double[] o1 = o[1];
- double[] o2 = o[2];
-
- // iterative correction: Xn+1 = Xn - 0.5 * (Xn.Mt.Xn - M)
- int i = 0;
- while (++i < 11) {
-
- // Mt.Xn
- double mx00 = m0[0] * x00 + m1[0] * x10 + m2[0] * x20;
- double mx10 = m0[1] * x00 + m1[1] * x10 + m2[1] * x20;
- double mx20 = m0[2] * x00 + m1[2] * x10 + m2[2] * x20;
- double mx01 = m0[0] * x01 + m1[0] * x11 + m2[0] * x21;
- double mx11 = m0[1] * x01 + m1[1] * x11 + m2[1] * x21;
- double mx21 = m0[2] * x01 + m1[2] * x11 + m2[2] * x21;
- double mx02 = m0[0] * x02 + m1[0] * x12 + m2[0] * x22;
- double mx12 = m0[1] * x02 + m1[1] * x12 + m2[1] * x22;
- double mx22 = m0[2] * x02 + m1[2] * x12 + m2[2] * x22;
-
- // Xn+1
- o0[0] = x00 - 0.5 * (x00 * mx00 + x01 * mx10 + x02 * mx20 - m0[0]);
- o0[1] = x01 - 0.5 * (x00 * mx01 + x01 * mx11 + x02 * mx21 - m0[1]);
- o0[2] = x02 - 0.5 * (x00 * mx02 + x01 * mx12 + x02 * mx22 - m0[2]);
- o1[0] = x10 - 0.5 * (x10 * mx00 + x11 * mx10 + x12 * mx20 - m1[0]);
- o1[1] = x11 - 0.5 * (x10 * mx01 + x11 * mx11 + x12 * mx21 - m1[1]);
- o1[2] = x12 - 0.5 * (x10 * mx02 + x11 * mx12 + x12 * mx22 - m1[2]);
- o2[0] = x20 - 0.5 * (x20 * mx00 + x21 * mx10 + x22 * mx20 - m2[0]);
- o2[1] = x21 - 0.5 * (x20 * mx01 + x21 * mx11 + x22 * mx21 - m2[1]);
- o2[2] = x22 - 0.5 * (x20 * mx02 + x21 * mx12 + x22 * mx22 - m2[2]);
-
- // correction on each elements
- double corr00 = o0[0] - m0[0];
- double corr01 = o0[1] - m0[1];
- double corr02 = o0[2] - m0[2];
- double corr10 = o1[0] - m1[0];
- double corr11 = o1[1] - m1[1];
- double corr12 = o1[2] - m1[2];
- double corr20 = o2[0] - m2[0];
- double corr21 = o2[1] - m2[1];
- double corr22 = o2[2] - m2[2];
-
- // Frobenius norm of the correction
- fn1 = corr00 * corr00 + corr01 * corr01 + corr02 * corr02 +
- corr10 * corr10 + corr11 * corr11 + corr12 * corr12 +
- corr20 * corr20 + corr21 * corr21 + corr22 * corr22;
-
- // convergence test
- if (Math.abs(fn1 - fn) <= threshold) {
- return o;
- }
-
- // prepare next iteration
- x00 = o0[0];
- x01 = o0[1];
- x02 = o0[2];
- x10 = o1[0];
- x11 = o1[1];
- x12 = o1[2];
- x20 = o2[0];
- x21 = o2[1];
- x22 = o2[2];
- fn = fn1;
-
- }
-
- // the algorithm did not converge after 10 iterations
- throw new IllegalArgumentException("Unable to orthogonalize matrix in " + (i - 1) + " iterations");
- }
-
- /** Compute the <i>distance</i> between two rotations.
- * <p>The <i>distance</i> is intended here as a way to check if two
- * rotations are almost similar (i.e. they transform vectors the same way)
- * or very different. It is mathematically defined as the angle of
- * the rotation r that prepended to one of the rotations gives the other
- * one:</p>
- * <div style="white-space: pre"><code>
- * r<sub>1</sub>(r) = r<sub>2</sub>
- * </code></div>
- * <p>This distance is an angle between 0 and π. Its value is the smallest
- * possible upper bound of the angle in radians between r<sub>1</sub>(v)
- * and r<sub>2</sub>(v) for all possible vectors v. This upper bound is
- * reached for some v. The distance is equal to 0 if and only if the two
- * rotations are identical.</p>
- * <p>Comparing two rotations should always be done using this value rather
- * than for example comparing the components of the quaternions. It is much
- * more stable, and has a geometric meaning. Also comparing quaternions
- * components is error prone since for example quaternions (0.36, 0.48, -0.48, -0.64)
- * and (-0.36, -0.48, 0.48, 0.64) represent exactly the same rotation despite
- * their components are different (they are exact opposites).</p>
- * @param r1 first rotation
- * @param r2 second rotation
- * @return <i>distance</i> between r1 and r2
- */
- public static double distance(Rotation r1, Rotation r2) {
- return r1.composeInverseInternal(r2).getAngle();
- }
-
- /** Exception thrown when an angle set encounters a singularity.
- */
- public static class AngleSetSingularityException extends GeometryException {
-
- /** Serializable version identifier */
- private static final long serialVersionUID = 20180913L;
-
- /** Simple constructor with an error message.
- * @param msg error message
- */
- public AngleSetSingularityException(String msg) {
- super(msg);
- }
- }
-
- /** Exception thrown when a Cardan angles singularity is encountered.
- */
- public static class CardanSingularityException extends AngleSetSingularityException {
-
- /** Serializable version identifier */
- private static final long serialVersionUID = 20180913L;
-
- /**
- * Simple constructor.
- */
- public CardanSingularityException() {
- super("Cardan angles singularity");
- }
- }
-
- /** Exception thrown when an Euler angles singularity is encountered.
- */
- public static class EulerSingularityException extends AngleSetSingularityException {
-
- /** Serializable version identifier */
- private static final long serialVersionUID = 20180913L;
-
- /**
- * Simple constructor.
- */
- public EulerSingularityException() {
- super("Euler angles singularity");
- }
- }
-}
diff --git a/commons-geometry-euclidean/src/main/java/org/apache/commons/geometry/euclidean/threed/RotationConvention.java b/commons-geometry-euclidean/src/main/java/org/apache/commons/geometry/euclidean/threed/RotationConvention.java
deleted file mode 100644
index 25abc95..0000000
--- a/commons-geometry-euclidean/src/main/java/org/apache/commons/geometry/euclidean/threed/RotationConvention.java
+++ /dev/null
@@ -1,78 +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.geometry.euclidean.threed;
-
-/**
- * This enumerates is used to differentiate the semantics of a rotation.
- * @see Rotation
- */
-public enum RotationConvention {
-
- /** Constant for rotation that have the semantics of a vector operator.
- * <p>
- * According to this convention, the rotation moves vectors with respect
- * to a fixed reference frame.
- * </p>
- * <p>
- * This means that if we define rotation r is a 90 degrees rotation around
- * the Z axis, the image of vector {@link Vector3D#PLUS_X} would be
- * {@link Vector3D#PLUS_Y}, the image of vector {@link Vector3D#PLUS_Y}
- * would be {@link Vector3D#MINUS_X}, the image of vector {@link Vector3D#PLUS_Z}
- * would be {@link Vector3D#PLUS_Z}, and the image of vector with coordinates (1, 2, 3)
- * would be vector (-2, 1, 3). This means that the vector rotates counterclockwise.
- * </p>
- * <p>
- * This convention was the only one supported by Apache Commons Math up to version 3.5.
- * </p>
- * <p>
- * The difference with {@link #FRAME_TRANSFORM} is only the semantics of the sign
- * of the angle. It is always possible to create or use a rotation using either
- * convention to really represent a rotation that would have been best created or
- * used with the other convention, by changing accordingly the sign of the
- * rotation angle. This is how things were done up to version 3.5.
- * </p>
- */
- VECTOR_OPERATOR,
-
- /** Constant for rotation that have the semantics of a frame conversion.
- * <p>
- * According to this convention, the rotation considered vectors to be fixed,
- * but their coordinates change as they are converted from an initial frame to
- * a destination frame rotated with respect to the initial frame.
- * </p>
- * <p>
- * This means that if we define rotation r is a 90 degrees rotation around
- * the Z axis, the image of vector {@link Vector3D#PLUS_X} would be
- * {@link Vector3D#MINUS_Y}, the image of vector {@link Vector3D#PLUS_Y}
- * would be {@link Vector3D#PLUS_X}, the image of vector {@link Vector3D#PLUS_Z}
- * would be {@link Vector3D#PLUS_Z}, and the image of vector with coordinates (1, 2, 3)
- * would be vector (2, -1, 3). This means that the coordinates of the vector rotates
- * clockwise, because they are expressed with respect to a destination frame that is rotated
- * counterclockwise.
- * </p>
- * <p>
- * The difference with {@link #VECTOR_OPERATOR} is only the semantics of the sign
- * of the angle. It is always possible to create or use a rotation using either
- * convention to really represent a rotation that would have been best created or
- * used with the other convention, by changing accordingly the sign of the
- * rotation angle. This is how things were done up to version 3.5.
- * </p>
- */
- FRAME_TRANSFORM;
-
-}
diff --git a/commons-geometry-euclidean/src/main/java/org/apache/commons/geometry/euclidean/threed/RotationOrder.java b/commons-geometry-euclidean/src/main/java/org/apache/commons/geometry/euclidean/threed/RotationOrder.java
deleted file mode 100644
index 0d0e440..0000000
--- a/commons-geometry-euclidean/src/main/java/org/apache/commons/geometry/euclidean/threed/RotationOrder.java
+++ /dev/null
@@ -1,172 +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.geometry.euclidean.threed;
-
-/**
- * This class is a utility representing a rotation order specification
- * for Cardan or Euler angles specification.
- *
- * This class cannot be instanciated by the user. He can only use one
- * of the twelve predefined supported orders as an argument to either
- * the {@link Rotation#Rotation(RotationOrder,double,double,double)}
- * constructor or the {@link Rotation#getAngles} method..2
- */
-public final class RotationOrder {
-
- /** Set of Cardan angles.
- * this ordered set of rotations is around X, then around Y, then
- * around Z
- */
- public static final RotationOrder XYZ =
- new RotationOrder("XYZ", Vector3D.PLUS_X, Vector3D.PLUS_Y, Vector3D.PLUS_Z);
-
- /** Set of Cardan angles.
- * this ordered set of rotations is around X, then around Z, then
- * around Y
- */
- public static final RotationOrder XZY =
- new RotationOrder("XZY", Vector3D.PLUS_X, Vector3D.PLUS_Z, Vector3D.PLUS_Y);
-
- /** Set of Cardan angles.
- * this ordered set of rotations is around Y, then around X, then
- * around Z
- */
- public static final RotationOrder YXZ =
- new RotationOrder("YXZ", Vector3D.PLUS_Y, Vector3D.PLUS_X, Vector3D.PLUS_Z);
-
- /** Set of Cardan angles.
- * this ordered set of rotations is around Y, then around Z, then
- * around X
- */
- public static final RotationOrder YZX =
- new RotationOrder("YZX", Vector3D.PLUS_Y, Vector3D.PLUS_Z, Vector3D.PLUS_X);
-
- /** Set of Cardan angles.
- * this ordered set of rotations is around Z, then around X, then
- * around Y
- */
- public static final RotationOrder ZXY =
- new RotationOrder("ZXY", Vector3D.PLUS_Z, Vector3D.PLUS_X, Vector3D.PLUS_Y);
-
- /** Set of Cardan angles.
- * this ordered set of rotations is around Z, then around Y, then
- * around X
- */
- public static final RotationOrder ZYX =
- new RotationOrder("ZYX", Vector3D.PLUS_Z, Vector3D.PLUS_Y, Vector3D.PLUS_X);
-
- /** Set of Euler angles.
- * this ordered set of rotations is around X, then around Y, then
- * around X
- */
- public static final RotationOrder XYX =
- new RotationOrder("XYX", Vector3D.PLUS_X, Vector3D.PLUS_Y, Vector3D.PLUS_X);
-
- /** Set of Euler angles.
- * this ordered set of rotations is around X, then around Z, then
- * around X
- */
- public static final RotationOrder XZX =
- new RotationOrder("XZX", Vector3D.PLUS_X, Vector3D.PLUS_Z, Vector3D.PLUS_X);
-
- /** Set of Euler angles.
- * this ordered set of rotations is around Y, then around X, then
- * around Y
- */
- public static final RotationOrder YXY =
- new RotationOrder("YXY", Vector3D.PLUS_Y, Vector3D.PLUS_X, Vector3D.PLUS_Y);
-
- /** Set of Euler angles.
- * this ordered set of rotations is around Y, then around Z, then
- * around Y
- */
- public static final RotationOrder YZY =
- new RotationOrder("YZY", Vector3D.PLUS_Y, Vector3D.PLUS_Z, Vector3D.PLUS_Y);
-
- /** Set of Euler angles.
- * this ordered set of rotations is around Z, then around X, then
- * around Z
- */
- public static final RotationOrder ZXZ =
- new RotationOrder("ZXZ", Vector3D.PLUS_Z, Vector3D.PLUS_X, Vector3D.PLUS_Z);
-
- /** Set of Euler angles.
- * this ordered set of rotations is around Z, then around Y, then
- * around Z
- */
- public static final RotationOrder ZYZ =
- new RotationOrder("ZYZ", Vector3D.PLUS_Z, Vector3D.PLUS_Y, Vector3D.PLUS_Z);
-
- /** Name of the rotations order. */
- private final String name;
-
- /** Axis of the first rotation. */
- private final Vector3D a1;
-
- /** Axis of the second rotation. */
- private final Vector3D a2;
-
- /** Axis of the third rotation. */
- private final Vector3D a3;
-
- /** Private constructor.
- * This is a utility class that cannot be instantiated by the user,
- * so its only constructor is private.
- * @param name name of the rotation order
- * @param a1 axis of the first rotation
- * @param a2 axis of the second rotation
- * @param a3 axis of the third rotation
- */
- private RotationOrder(final String name,
- final Vector3D a1, final Vector3D a2, final Vector3D a3) {
- this.name = name;
- this.a1 = a1;
- this.a2 = a2;
- this.a3 = a3;
- }
-
- /** Get a string representation of the instance.
- * @return a string representation of the instance (in fact, its name)
- */
- @Override
- public String toString() {
- return name;
- }
-
- /** Get the axis of the first rotation.
- * @return axis of the first rotation
- */
- public Vector3D getA1() {
- return a1;
- }
-
- /** Get the axis of the second rotation.
- * @return axis of the second rotation
- */
- public Vector3D getA2() {
- return a2;
- }
-
- /** Get the axis of the second rotation.
- * @return axis of the second rotation
- */
- public Vector3D getA3() {
- return a3;
- }
-
-}
diff --git a/commons-geometry-euclidean/src/main/java/org/apache/commons/geometry/euclidean/threed/Vector3D.java b/commons-geometry-euclidean/src/main/java/org/apache/commons/geometry/euclidean/threed/Vector3D.java
index 4440884..2add39c 100644
--- a/commons-geometry-euclidean/src/main/java/org/apache/commons/geometry/euclidean/threed/Vector3D.java
+++ b/commons-geometry-euclidean/src/main/java/org/apache/commons/geometry/euclidean/threed/Vector3D.java
@@ -360,6 +360,16 @@ public class Vector3D extends MultiDimensionalEuclideanVector<Vector3D> {
LinearCombination.value(x, v.y, -y, v.x));
}
+ /** Apply the given transform to this vector, returning the result as a
+ * new vector instance.
+ * @param transform the transform to apply
+ * @return a new, transformed vector
+ * @see AffineTransformMatrix3D#apply(Vector3D)
+ */
+ public Vector3D transform(AffineTransformMatrix3D transform) {
+ return transform.apply(this);
+ }
+
/**
* Get a hashCode for the vector.
* <p>All NaN values have the same hash code.</p>
diff --git a/commons-geometry-euclidean/src/main/java/org/apache/commons/geometry/euclidean/threed/rotation/AxisAngleSequence.java b/commons-geometry-euclidean/src/main/java/org/apache/commons/geometry/euclidean/threed/rotation/AxisAngleSequence.java
new file mode 100644
index 0000000..f59a76f
--- /dev/null
+++ b/commons-geometry-euclidean/src/main/java/org/apache/commons/geometry/euclidean/threed/rotation/AxisAngleSequence.java
@@ -0,0 +1,210 @@
+/*
+ * 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.geometry.euclidean.threed.rotation;
+
+import java.io.Serializable;
+import java.util.Arrays;
+import java.util.Objects;
+
+import org.apache.commons.numbers.core.Precision;
+
+/** <p>
+ * Class representing a sequence of axis-angle rotations. These types of
+ * rotations are commonly called <em>Euler angles</em>, <em>Tait-Bryan angles</em>,
+ * or <em>Cardan angles</em> depending on the properties of the rotation sequence and
+ * the particular use case. A sequence of three rotations around at least two different
+ * axes is sufficient to represent any rotation or orientation in 3 dimensional space.
+ * However, in order to unambiguously represent the rotation, the following information
+ * must be provided along with the rotation angles:
+ * <ul>
+ * <li><strong>Axis sequence</strong> - The axes that the rotation angles are associated with and
+ * in what order they occur.
+ * </li>
+ * <li><strong>Reference frame</strong> - The reference frame used to define the position of the rotation
+ * axes. This can either be <em>relative (intrinsic)</em> or <em>absolute (extrinsic)</em>. A relative
+ * reference frame defines the rotation axes from the point of view of the "thing" being rotated.
+ * Thus, each rotation after the first occurs around an axis that very well may have been
+ * moved from its original position by a previous rotation. A good example of this is an
+ * airplane: the pilot steps through a sequence of rotations, each time moving the airplane
+ * around its own up/down, left/right, and front/back axes, regardless of how the airplane
+ * is oriented at the time. In contrast, an absolute reference frame is fixed and does not
+ * move with each rotation.
+ * </li>
+ * <li><strong>Rotation direction</strong> - This defines the rotation direction that angles are measured in.
+ * This library uses <em>right-handed rotations</em> exclusively. This means that the direction of rotation
+ * around an axis is the same as the curl of one's fingers when the right hand is placed on the axis
+ * with the thumb pointing in the axis direction.
+ * </li>
+ * </ul>
+ *
+ * <p>
+ * Computations involving multiple rotations are generally very complicated when using axis-angle sequences. Therefore, it is recommended
+ * to only use this class to represent angles and orientations when needed in this form, and to use {@link QuaternionRotation}
+ * for everything else. Quaternions are much easier to work with and avoid many of the problems of axis-angle sequence representations,
+ * such as <a href="https://en.wikipedia.org/wiki/Gimbal_lock">gimbal lock</a>.
+ * </p>
+ *
+ * @see <a href="https://en.wikipedia.org/wiki/Euler_angles">Euler Angles</a>
+ * @see QuaternionRotation
+ */
+public final class AxisAngleSequence implements Serializable {
+
+ /** Serializable identifier*/
+ private static final long serialVersionUID = 20181125L;
+
+ /** Reference frame for defining axis positions. */
+ private final AxisReferenceFrame referenceFrame;
+
+ /** Axis sequence. */
+ private final AxisSequence axisSequence;
+
+ /** Angle around the first rotation axis, in radians. */
+ private final double angle1;
+
+ /** Angle around the second rotation axis, in radians. */
+ private final double angle2;
+
+ /** Angle around the third rotation axis, in radians. */
+ private final double angle3;
+
+ /** Construct an instance from its component parts.
+ * @param referenceFrame the axis reference frame
+ * @param axisSequence the axis rotation sequence
+ * @param angle1 angle around the first axis in radians
+ * @param angle2 angle around the second axis in radians
+ * @param angle3 angle around the third axis in radians
+ */
+ public AxisAngleSequence(final AxisReferenceFrame referenceFrame, final AxisSequence axisSequence, final double angle1,
+ final double angle2, final double angle3) {
+ this.referenceFrame = referenceFrame;
+ this.axisSequence = axisSequence;
+
+ this.angle1 = angle1;
+ this.angle2 = angle2;
+ this.angle3 = angle3;
+ }
+
+ /** Get the axis reference frame. This defines the position of the rotation axes.
+ * @return the axis reference frame
+ */
+ public AxisReferenceFrame getReferenceFrame() {
+ return referenceFrame;
+ }
+
+ /** Get the rotation axis sequence.
+ * @return the rotation axis sequence
+ */
+ public AxisSequence getAxisSequence() {
+ return axisSequence;
+ }
+
+ /** Get the angle of rotation around the first axis, in radians.
+ * @return angle of rotation around the first axis, in radians
+ */
+ public double getAngle1() {
+ return angle1;
+ }
+
+ /** Get the angle of rotation around the second axis, in radians.
+ * @return angle of rotation around the second axis, in radians
+ */
+ public double getAngle2() {
+ return angle2;
+ }
+
+ /** Get the angle of rotation around the thrid axis, in radians.
+ * @return angle of rotation around the thrid axis, in radians
+ */
+ public double getAngle3() {
+ return angle3;
+ }
+
+ /** Get the rotation angles as a 3-element array.
+ * @return an array containing the 3 rotation angles
+ */
+ public double[] getAngles() {
+ return new double[] { angle1, angle2, angle3 };
+ }
+
+ /** {@inheritDoc} */
+ @Override
+ public int hashCode() {
+ return 107 * (199 * Objects.hash(referenceFrame, axisSequence)) +
+ (7 * Double.hashCode(angle1)) +
+ (11 * Double.hashCode(angle2)) +
+ (19 * Double.hashCode(angle3));
+ }
+
+ /** {@inheritDoc} */
+ @Override
+ public boolean equals(Object obj) {
+ if (this == obj) {
+ return true;
+ }
+ if (!(obj instanceof AxisAngleSequence)) {
+ return false;
+ }
+
+ final AxisAngleSequence other = (AxisAngleSequence) obj;
+
+ return this.referenceFrame == other.referenceFrame &&
+ this.axisSequence == other.axisSequence &&
+ Precision.equals(this.angle1, other.angle1) &&
+ Precision.equals(this.angle2, other.angle2) &&
+ Precision.equals(this.angle3, other.angle3);
+ }
+
+ /** {@inheritDoc} */
+ @Override
+ public String toString() {
+ final StringBuilder sb = new StringBuilder();
+ sb.append(this.getClass().getSimpleName())
+ .append("[referenceFrame=")
+ .append(referenceFrame)
+ .append(", axisSequence=")
+ .append(axisSequence)
+ .append(", angles=")
+ .append(Arrays.toString(getAngles()))
+ .append(']');
+
+ return sb.toString();
+ }
+
+ /** Create a new instance with a reference frame of {@link AxisReferenceFrame#RELATIVE}.
+ * @param axisSequence the axis rotation sequence
+ * @param angle1 angle around the first axis in radians
+ * @param angle2 angle around the second axis in radians
+ * @param angle3 angle around the third axis in radians
+ * @return a new instance with a relative reference frame
+ */
+ public static AxisAngleSequence createRelative(final AxisSequence axisSequence, final double angle1,
+ final double angle2, final double angle3) {
+ return new AxisAngleSequence(AxisReferenceFrame.RELATIVE, axisSequence, angle1, angle2, angle3);
+ }
+
+ /** Create a new instance with a reference frame of {@link AxisReferenceFrame#ABSOLUTE}.
+ * @param axisSequence the axis rotation sequence
+ * @param angle1 angle around the first axis in radians
+ * @param angle2 angle around the second axis in radians
+ * @param angle3 angle around the third axis in radians
+ * @return a new instance with an absolute reference frame
+ */
+ public static AxisAngleSequence createAbsolute(final AxisSequence axisSequence, final double angle1,
+ final double angle2, final double angle3) {
+ return new AxisAngleSequence(AxisReferenceFrame.ABSOLUTE, axisSequence, angle1, angle2, angle3);
+ }
+}
diff --git a/commons-geometry-euclidean/src/main/java/org/apache/commons/geometry/euclidean/threed/rotation/AxisReferenceFrame.java b/commons-geometry-euclidean/src/main/java/org/apache/commons/geometry/euclidean/threed/rotation/AxisReferenceFrame.java
new file mode 100644
index 0000000..68cf7ee
--- /dev/null
+++ b/commons-geometry-euclidean/src/main/java/org/apache/commons/geometry/euclidean/threed/rotation/AxisReferenceFrame.java
@@ -0,0 +1,55 @@
+/*
+ * 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.geometry.euclidean.threed.rotation;
+
+/** Enum defining the possible reference frames for locating axis
+ * positions during a rotation sequence.
+ */
+public enum AxisReferenceFrame {
+
+ /** Defines a relative reference frame for a rotation sequence. Sequences
+ * with this type of reference frame are called <em>intrinsic rotations</em>.
+ *
+ * <p>
+ * When using a relative reference frame, each successive axis
+ * is located relative to the "thing" being rotated and not to some
+ * external frame of reference. For example, say that a rotation sequence
+ * is defined around the {@code x}, {@code y}, and {@code z} axes in
+ * that order. The first rotation will occur around the standard {@code x}
+ * axis. The second rotation, however, will occur around the {@code y}
+ * axis after it has been rotated by the first rotation; we can call this
+ * new axis {@code y'}. Similarly, the third rotation will occur around
+ * {@code z''}, which may or may not match the original {@code z} axis.
+ * A good real-world example of this type of situation is an airplane,
+ * where a pilot makes a sequence of rotations in order, with each rotation
+ * using the airplane's own up/down, left/right, back/forward directions
+ * as the frame of reference.
+ * </p>
+ */
+ RELATIVE,
+
+ /** Defines an absolute reference frame for a rotation sequence. Sequences
+ * with this type of reference frame are called <em>extrinsic rotations</em>.
+ *
+ * <p>
+ * In contrast with the relative reference frame, the absolute reference frame
+ * remains fixed throughout a rotation sequence, with each rotation axis not
+ * affected by the rotations.
+ * </p>
+ */
+ ABSOLUTE
+}
diff --git a/commons-geometry-euclidean/src/main/java/org/apache/commons/geometry/euclidean/threed/rotation/AxisSequence.java b/commons-geometry-euclidean/src/main/java/org/apache/commons/geometry/euclidean/threed/rotation/AxisSequence.java
new file mode 100644
index 0000000..68c8fde
--- /dev/null
+++ b/commons-geometry-euclidean/src/main/java/org/apache/commons/geometry/euclidean/threed/rotation/AxisSequence.java
@@ -0,0 +1,146 @@
+/*
+ * 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.geometry.euclidean.threed.rotation;
+
+import org.apache.commons.geometry.euclidean.threed.Vector3D;
+
+/** Enum containing rotation axis sequences for use in defining 3 dimensional rotations.
+ */
+public enum AxisSequence {
+
+ /** Set of Tait-Bryan angles around the <strong>X</strong>, <strong>Y</strong>, and
+ * <strong>Z</strong> axes in that order.
+ */
+ XYZ(AxisSequenceType.TAIT_BRYAN, Vector3D.PLUS_X, Vector3D.PLUS_Y, Vector3D.PLUS_Z),
+
+ /** Set of Tait-Bryan angles around the <strong>X</strong>, <strong>Z</strong>, and
+ * <strong>Y</strong> axes in that order.
+ */
+ XZY(AxisSequenceType.TAIT_BRYAN, Vector3D.PLUS_X, Vector3D.PLUS_Z, Vector3D.PLUS_Y),
+
+ /** Set of Tait-Bryan angles around the <strong>Y</strong>, <strong>X</strong>, and
+ * <strong>Z</strong> axes in that order.
+ */
+ YXZ(AxisSequenceType.TAIT_BRYAN, Vector3D.PLUS_Y, Vector3D.PLUS_X, Vector3D.PLUS_Z),
+
+ /** Set of Tait-Bryan angles around the <strong>Y</strong>, <strong>Z</strong>, and
+ * <strong>X</strong> axes in that order.
+ */
+ YZX(AxisSequenceType.TAIT_BRYAN, Vector3D.PLUS_Y, Vector3D.PLUS_Z, Vector3D.PLUS_X),
+
+ /** Set of Cardan angles.
+ * this ordered set of rotations is around Z, then around X, then
+ * around Y
+ */
+ ZXY(AxisSequenceType.TAIT_BRYAN, Vector3D.PLUS_Z, Vector3D.PLUS_X, Vector3D.PLUS_Y),
+
+ /** Set of Tait-Bryan angles around the <strong>Z</strong>, <strong>Y</strong>, and
+ * <strong>X</strong> axes in that order.
+ */
+ ZYX(AxisSequenceType.TAIT_BRYAN, Vector3D.PLUS_Z, Vector3D.PLUS_Y, Vector3D.PLUS_X),
+
+ /** Set of Euler angles around the <strong>X</strong>, <strong>Y</strong>, and
+ * <strong>X</strong> axes in that order.
+ */
+ XYX(AxisSequenceType.EULER, Vector3D.PLUS_X, Vector3D.PLUS_Y, Vector3D.PLUS_X),
+
+ /** Set of Euler angles around the <strong>X</strong>, <strong>Z</strong>, and
+ * <strong>X</strong> axes in that order.
+ */
+ XZX(AxisSequenceType.EULER, Vector3D.PLUS_X, Vector3D.PLUS_Z, Vector3D.PLUS_X),
+
+ /** Set of Euler angles around the <strong>Y</strong>, <strong>X</strong>, and
+ * <strong>Y</strong> axes in that order.
+ */
+ YXY(AxisSequenceType.EULER, Vector3D.PLUS_Y, Vector3D.PLUS_X, Vector3D.PLUS_Y),
+
+ /** Set of Euler angles around the <strong>Y</strong>, <strong>Z</strong>, and
+ * <strong>Y</strong> axes in that order.
+ */
+ YZY(AxisSequenceType.EULER, Vector3D.PLUS_Y, Vector3D.PLUS_Z, Vector3D.PLUS_Y),
+
+ /** Set of Euler angles around the <strong>Z</strong>, <strong>X</strong>, and
+ * <strong>Z</strong> axes in that order.
+ */
+ ZXZ(AxisSequenceType.EULER, Vector3D.PLUS_Z, Vector3D.PLUS_X, Vector3D.PLUS_Z),
+
+ /** Set of Euler angles around the <strong>Z</strong>, <strong>Y</strong>, and
+ * <strong>Z</strong> axes in that order.
+ */
+ ZYZ(AxisSequenceType.EULER, Vector3D.PLUS_Z, Vector3D.PLUS_Y, Vector3D.PLUS_Z);
+
+ /** The type of axis sequence. */
+ private final AxisSequenceType type;
+
+ /** Axis of the first rotation. */
+ private final Vector3D axis1;
+
+ /** Axis of the second rotation. */
+ private final Vector3D axis2;
+
+ /** Axis of the third rotation. */
+ private final Vector3D axis3;
+
+ /** Simple constructor
+ * @param type the axis sequence type
+ * @param axis1 first rotation axis
+ * @param axis2 second rotation axis
+ * @param axis3 third rotation axis
+ */
+ AxisSequence(final AxisSequenceType type, final Vector3D axis1, final Vector3D axis2, final Vector3D axis3) {
+ this.type = type;
+
+ this.axis1 = axis1;
+ this.axis2 = axis2;
+ this.axis3 = axis3;
+ }
+
+ /** Get the axis sequence type.
+ * @return the axis sequence type
+ */
+ public AxisSequenceType getType() {
+ return type;
+ }
+
+ /** Get the first rotation axis.
+ * @return the first rotation axis
+ */
+ public Vector3D getAxis1() {
+ return axis1;
+ }
+
+ /** Get the second rotation axis.
+ * @return the second rotation axis
+ */
+ public Vector3D getAxis2() {
+ return axis2;
+ }
+
+ /** Get the third rotation axis.
+ * @return the third rotation axis
+ */
+ public Vector3D getAxis3() {
+ return axis3;
+ }
+
+ /** Get an array containing the 3 rotation axes in order.
+ * @return a 3-element array containing the rotation axes in order
+ */
+ public Vector3D[] toArray() {
+ return new Vector3D[] { axis1, axis2, axis3 };
+ }
+}
diff --git a/commons-geometry-euclidean/src/main/java/org/apache/commons/geometry/euclidean/threed/rotation/AxisSequenceType.java b/commons-geometry-euclidean/src/main/java/org/apache/commons/geometry/euclidean/threed/rotation/AxisSequenceType.java
new file mode 100644
index 0000000..762d34f
--- /dev/null
+++ b/commons-geometry-euclidean/src/main/java/org/apache/commons/geometry/euclidean/threed/rotation/AxisSequenceType.java
@@ -0,0 +1,38 @@
+/*
+ * 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.geometry.euclidean.threed.rotation;
+
+/** Defines different types of rotation axis sequences.
+ */
+public enum AxisSequenceType {
+
+ /** Represents Euler angles, which consist of axis sequences
+ * in the pattern <em>ABA</em>. For example, the sequences {@code ZXZ}, {@code XYX}, etc.
+ * fit this definition. Other types of sequences that do not match this
+ * pattern are often called "Euler angles" in common usage. However, this enum
+ * value is intended to represent only those sequences that match exactly, ie "proper" Euler angles.
+ * @see <a href="https://en.wikipedia.org/wiki/Euler_angles#Proper_Euler_angles">Proper Euler angles</a>
+ */
+ EULER,
+
+ /** Represents Tait-Bryan angles, which consist of axis sequences
+ * in the pattern <em>ABC</em>. For example, the sequences {@code XYZ}, {@code ZXY},
+ * etc. fit this definition. Tait-Bryan angles are also called Cardan angles.
+ * @see <a href="https://en.wikipedia.org/wiki/Euler_angles#Tait%E2%80%93Bryan_angles">Tait-Bryan angles</a>
+ */
+ TAIT_BRYAN
+}
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
new file mode 100644
index 0000000..0297779
--- /dev/null
+++ b/commons-geometry-euclidean/src/main/java/org/apache/commons/geometry/euclidean/threed/rotation/QuaternionRotation.java
@@ -0,0 +1,980 @@
+/*
+ * 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.geometry.euclidean.threed.rotation;
+
+import java.io.Serializable;
+
+import org.apache.commons.geometry.core.Geometry;
+import org.apache.commons.geometry.core.exception.IllegalNormException;
+import org.apache.commons.geometry.core.internal.GeometryInternalError;
+import org.apache.commons.geometry.core.internal.SimpleTupleFormat;
+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;
+
+/**
+ * Class representing a unit-length quaternion used to model rotations in 3 dimensional
+ * Euclidean space.
+ *
+ * @see <a href="https://en.wikipedia.org/wiki/Quaternions_and_spatial_rotation">Quaternion Rotations</a>
+ */
+public final class QuaternionRotation implements Serializable {
+
+ /** Serializable version identifier */
+ private static final long serialVersionUID = 20181018L;
+
+ /** Threshold max value for the dot product computed in the {@code #slerp(QuaternionRotation, double)}
+ * method. If the quaternion dot product is greater than this value (ie, the
+ * quaternions are very close to each other), then the quaternions are
+ * linearly interpolated instead of spherically interpolated.
+ */
+ private static final double SLERP_MAX_DOT_THRESHOLD = 0.9995;
+
+ /** Threshold value for the dot product of antiparallel vectors. If the dot product of two vectors is
+ * less than this value, (adjusted for the lengths of the vectors), then the vectors are considered to be
+ * antiparallel (ie, negations of each other).
+ */
+ private static final double ANTIPARALLEL_DOT_THRESHOLD = 2.0e-15 - 1.0;
+
+ /** Threshold value used to identify singularities when converting from quaternions to
+ * axis angle sequences.
+ */
+ private static final double AXIS_ANGLE_SINGULARITY_THRESHOLD = 0.9999999999;
+
+ /** 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);
+
+ /** First coordinate of the vectorial part of the quaternion. */
+ private final double qx;
+
+ /** 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. 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]}.
+ *
+ * @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;
+ }
+ }
+
+ /**
+ * Get the first vectorial coordinate of the quaternion.
+ *
+ * @return first vectorial coordinate of the quaternion
+ */
+ public double getX() {
+ return qx;
+ }
+
+ /**
+ * Get the second vectorial coordinate of the quaternion.
+ *
+ * @return second vectorial coordinate of the quaternion
+ */
+ public double getY() {
+ return qy;
+ }
+
+ /**
+ * Get the third vectorial coordinate of the quaternion.
+ *
+ * @return third vectorial coordinate of the quaternion
+ */
+ public double getZ() {
+ return qz;
+ }
+
+ /**
+ * Get the scalar coordinate of the quaternion.
+ *
+ * @return scalar coordinate of the quaternion
+ */
+ public double getW() {
+ return qw;
+ }
+
+ /**
+ * Get the quaternion coordinates as an array in the order
+ * {@code [x, y, z, w]}.
+ *
+ * @return the quaternion coordinates in the order {@code [x, y, z, w]}
+ */
+ public double[] toArray() {
+ return new double[] { qx, qy, qz, qw };
+ }
+
+ /**
+ * Get the axis of rotation as a normalized {@link Vector3D}. The rotation axis
+ * is not well defined when the rotation is the identity rotation, ie it has a
+ * rotation angle of zero. In this case, the vector representing the positive
+ * x-axis is returned.
+ *
+ * @return the axis of rotation
+ */
+ public Vector3D getAxis() {
+ // 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);
+ }
+ catch (IllegalNormException exc) {
+ return Vector3D.PLUS_X;
+ }
+ }
+
+ /**
+ * Get the angle of rotation in radians. The returned value is in the range 0
+ * through {@code pi}.
+ *
+ * @return The rotation angle in the range {@code [0, pi]}.
+ */
+ public double getAngle() {
+ return 2 * Math.acos(qw);
+ }
+
+ /**
+ * Get the inverse of this rotation. The returned rotation has the same
+ * rotation angle but the opposite rotation axis. If {@code r.apply(u)}
+ * is equal to {@code v}, then {@code r.negate().apply(v)} is equal
+ * to {@code u}.
+ *
+ * @return the negation (inverse) of the rotation
+ */
+ public QuaternionRotation getInverse() {
+ return new QuaternionRotation(-qx, -qy, -qz, qw);
+ }
+
+ /**
+ * Apply this rotation to the given vector.
+ *
+ * @param v vector to rotate
+ * @return the rotated vector
+ */
+ public Vector3D apply(final Vector3D v) {
+ 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 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
+
+ return Vector3D.of(
+ (iw * -qx) + (ix * qw) + (iy * -qz) - (iz * -qy),
+ (iw * -qy) - (ix * -qz) + (iy * qw) + (iz * -qx),
+ (iw * -qz) + (ix * -qy) - (iy * -qx) + (iz * qw)
+ );
+ }
+
+ /**
+ * Multiply this instance by the given argument, returning the result as
+ * a new instance. This is equivalent to the expression {@code t * q} where
+ * {@code q} is the argument and {@code t} is this instance.
+ *
+ * <p>
+ * Multiplication of quaternions behaves similarly to transformation
+ * matrices in regard to the order that operations are performed.
+ * For example, if <code>q<sub>1</sub></code> and <code>q<sub>2</sub></code> are unit
+ * quaternions, then the quaternion <code>q<sub>r</sub> = q<sub>1</sub>*q<sub>2</sub></code>
+ * will give the effect of applying the rotation in <code>q<sub>2</sub></code> followed
+ * by the rotation in <code>q<sub>1</sub></code>. In other words, the rightmost element
+ * in the multiplication is applied first.
+ * </p>
+ *
+ * @param q quaternion to multiply with the current instance
+ * @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);
+ }
+
+ /** Multiply the argument by this instance, returning the result as
+ * a new instance. This is equivalent to the expression {@code q * t} where
+ * {@code q} is the argument and {@code t} is this instance.
+ *
+ * <p>
+ * Multiplication of quaternions behaves similarly to transformation
+ * matrices in regard to the order that operations are performed.
+ * For example, if <code>q<sub>1</sub></code> and <code>q<sub>2</sub></code> are unit
+ * quaternions, then the quaternion <code>q<sub>r</sub> = q<sub>1</sub>*q<sub>2</sub></code>
+ * will give the effect of applying the rotation in <code>q<sub>2</sub></code> followed
+ * by the rotation in <code>q<sub>1</sub></code>. In other words, the rightmost element
+ * in the multiplication is applied first.
+ * </p>
+ *
+ * @param q quaternion to multiply by the current instance
+ * @return the result of multiplying the argument by the current instance
+ */
+ public QuaternionRotation premultiply(final QuaternionRotation q) {
+ return q.multiply(this);
+ }
+
+ /** Perform spherical linear interpolation (Slerp) between the current instance and {@code end}.
+ *
+ * <p>
+ * Slerp is an algorithm designed to interpolate smoothly between two rotations/orientations,
+ * producing a constant-speed motion along an arc. The original purpose of this algorithm was
+ * to animate 3D rotations. The rotation returned by this method is controlled by the interpolation parameter,
+ * {@code t}. When {@code t = 0}, a rotation equal to the current instance is
+ * returned. When {@code t = 1}, a rotation equal to {@code end} is returned. All other values
+ * are interpolated (or extrapolated if {@code t} is outside of the {@code [0, 1]} range).
+ * </p>
+ *
+ * @param end the end rotation of the interpolation.
+ * @param t interpolation parameter
+ * @return rotation representing a spherical linear interpolation between this instance and
+ * {@code end}
+ *
+ * @see <a href="https://en.wikipedia.org/wiki/Slerp">Slerp</a>
+ */
+ public QuaternionRotation slerp(final QuaternionRotation end, final double t)
+ {
+ // reference formula:
+ // https://en.wikipedia.org/wiki/Slerp
+
+ // handle no-op cases
+ if (t == 0) {
+ return this;
+ }
+ else if (t == 1.0) {
+ return end;
+ }
+
+ double endX = end.qx;
+ double endY = end.qy;
+ double endZ = end.qz;
+ double endW = end.qw;
+
+ double dot = dotProduct(end);
+
+ // 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
+ // to produce an equivalent rotation that will give us the path we want.
+ if (dot < 0.0) {
+ dot = -dot;
+
+ endX = -endX;
+ endY = -endY;
+ endZ = -endZ;
+ endW = -endW;
+ }
+
+ // If the quaternions are too closely aligned, perform linear interpolation
+ // and normalize the result. Otherwise, we may end up dividing by zero with the
+ // 1/sin(theta) term below.
+ if (dot > SLERP_MAX_DOT_THRESHOLD) {
+ 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));
+ }
+
+ // compute the standard slerp formula
+ final double theta = Math.acos(dot);
+ final double sinTheta = Math.sin(theta);
+
+ 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 a {@link AffineTransformMatrix3D} that performs the rotation
+ * represented by this instance.
+ *
+ * @return an {@link AffineTransformMatrix3D} instance that performs the rotation
+ * represented by this instance
+ */
+ public AffineTransformMatrix3D toTransformMatrix() {
+ // pre-calculate products that we'll need
+ final double xx = qx * qx;
+ final double xy = qx * qy;
+ final double xz = qx * qz;
+ final double xw = qx * qw;
+
+ final double yy = qy * qy;
+ final double yz = qy * qz;
+ final double yw = qy * qw;
+
+ final double zz = qz * qz;
+ final double zw = qz * qw;
+
+ final double m00 = 1.0 - (2.0 * (yy + zz));
+ final double m01 = 2.0 * (xy - zw);
+ final double m02 = 2.0 * (xz + yw);
+ final double m03 = 0.0;
+
+ final double m10 = 2.0 * (xy + zw);
+ final double m11 = 1.0 - (2.0 * (xx + zz));
+ final double m12 = 2.0 * (yz - xw);
+ final double m13 = 0.0;
+
+ final double m20 = 2.0 * (xz - yw);
+ final double m21 = 2.0 * (yz + xw);
+ final double m22 = 1.0 - (2.0 * (xx + yy));
+ final double m23 = 0.0;
+
+ return AffineTransformMatrix3D.of(
+ m00, m01, m02, m03,
+ m10, m11, m12, m13,
+ m20, m21, m22, m23
+ );
+ }
+
+ /** Get a sequence of axis-angle rotations that produce an overall rotation equivalent to this instance.
+ *
+ * <p>
+ * In most cases, the returned rotation sequence will be unique. However, at points of singularity
+ * (second angle equal to {@code 0} or {@code -pi} for Euler angles and {@code +pi/2} or {@code -pi/2}
+ * for Tait-Bryan angles), there are an infinite number of possible sequences that produce the same result.
+ * In these cases, the result is returned that leaves the last rotation equal to 0 (in the case of a relative
+ * reference frame) or the first rotation equal to 0 (in the case of an absolute reference frame).
+ * </p>
+ *
+ * @param frame the reference frame used to interpret the positions of the rotation axes
+ * @param axes the sequence of rotation axes
+ * @return a sequence of axis-angle rotations equivalent to this rotation
+ */
+ public AxisAngleSequence toAxisAngleSequence(final AxisReferenceFrame frame, final AxisSequence axes) {
+ if (frame == null) {
+ throw new IllegalArgumentException("Axis reference frame cannot be null");
+ }
+ if (axes == null) {
+ throw new IllegalArgumentException("Axis sequence cannot be null");
+ }
+
+ double[] angles = getAngles(frame, axes);
+
+ return new AxisAngleSequence(frame, axes, angles[0], angles[1], angles[2]);
+ }
+
+ /** Get a sequence of axis-angle rotations that produce an overall rotation equivalent to this instance.
+ * Each rotation axis is interpreted relative to the rotated coordinate frame (ie, intrinsic rotation).
+ * @param axes the sequence of rotation axes
+ * @return a sequence of relative axis-angle rotations equivalent to this rotation
+ * @see #toAxisAngleSequence(AxisReferenceFrame, AxisSequence)
+ */
+ public AxisAngleSequence toRelativeAxisAngleSequence(final AxisSequence axes) {
+ return toAxisAngleSequence(AxisReferenceFrame.RELATIVE, axes);
+ }
+
+ /** Get a sequence of axis-angle rotations that produce an overall rotation equivalent to this instance.
+ * Each rotation axis is interpreted as part of an absolute, unmoving coordinate frame (ie, extrinsic rotation).
+ * @param axes the sequence of rotation axes
+ * @return a sequence of absolute axis-angle rotations equivalent to this rotation
+ * @see #toAxisAngleSequence(AxisReferenceFrame, AxisSequence)
+ */
+ public AxisAngleSequence toAbsoluteAxisAngleSequence(final AxisSequence axes) {
+ return toAxisAngleSequence(AxisReferenceFrame.ABSOLUTE, axes);
+ }
+
+ /** {@inheritDoc} */
+ @Override
+ public int hashCode() {
+ return 131 * (193 * Double.hashCode(qx)) +
+ (3 * Double.hashCode(qy)) +
+ (5 * Double.hashCode(qz)) +
+ (19 * Double.hashCode(qw));
+ }
+
+ /** {@inheritDoc} */
+ @Override
+ public boolean equals(Object obj) {
+ if (this == obj) {
+ return true;
+ }
+ if (!(obj instanceof QuaternionRotation)) {
+ return false;
+ }
+
+ 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);
+ }
+
+ /** {@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);
+ }
+
+ /** Get a sequence of angles around the given axes that produce a rotation equivalent
+ * to this instance.
+ * @param frame the reference frame used to define the positions of the axes
+ * @param axes the axis sequence
+ * @return a sequence of angles around the given axes that produce a rotation equivalent
+ * to this instance
+ */
+ private double[] getAngles(final AxisReferenceFrame frame, final AxisSequence axes) {
+
+ AxisSequenceType sequenceType = axes.getType();
+
+ final Vector3D axis1 = axes.getAxis1();
+ final Vector3D axis2 = axes.getAxis2();
+ final Vector3D axis3 = axes.getAxis3();
+
+ if (frame == AxisReferenceFrame.RELATIVE) {
+ if (sequenceType == AxisSequenceType.TAIT_BRYAN) {
+ return getRelativeTaitBryanAngles(axis1, axis2, axis3);
+ }
+ else if (sequenceType == AxisSequenceType.EULER) {
+ return getRelativeEulerAngles(axis1, axis2, axis3);
+ }
+ }
+ else if (frame == AxisReferenceFrame.ABSOLUTE) {
+ if (sequenceType == AxisSequenceType.TAIT_BRYAN) {
+ return getAbsoluteTaitBryanAngles(axis1, axis2, axis3);
+ }
+ else if (sequenceType == AxisSequenceType.EULER) {
+ return getAbsoluteEulerAngles(axis1, axis2, axis3);
+ }
+ }
+
+ // all possibilities should have been covered above
+ throw new GeometryInternalError();
+ }
+
+ /** Get a sequence of angles around the given Tait-Bryan axes that produce a rotation equivalent
+ * to this instance. The axes are interpreted as being relative to the rotated coordinate frame.
+ * @param axis1 first Tait-Bryan axis
+ * @param axis2 second Tait-Bryan axis
+ * @param axis3 third Tait-Bryan axis
+ * @return a sequence of rotation angles around the relative input axes that produce a rotation equivalent
+ * to this instance
+ */
+ private double[] getRelativeTaitBryanAngles(final Vector3D axis1, final Vector3D axis2, final Vector3D axis3) {
+
+ // We can use geometry to get the first and second angles pretty easily here by analyzing the positions
+ // of the transformed rotation axes. The third angle is trickier but we can get it by treating it as
+ // if it were the first rotation in the inverse (which it would be).
+
+ final Vector3D vec3 = apply(axis3);
+ final Vector3D invVec1 = getInverse().apply(axis1);
+
+ final double angle2Sin = vec3.dotProduct(axis2.crossProduct(axis3));
+
+ if (angle2Sin < -AXIS_ANGLE_SINGULARITY_THRESHOLD ||
+ angle2Sin > AXIS_ANGLE_SINGULARITY_THRESHOLD) {
+
+ final Vector3D vec2 = apply(axis2);
+
+ final double angle1TanY = vec2.dotProduct(axis1.crossProduct(axis2));
+ final double angle1TanX = vec2.dotProduct(axis2);
+
+ final double angle2 = angle2Sin > AXIS_ANGLE_SINGULARITY_THRESHOLD ? Geometry.HALF_PI : Geometry.MINUS_HALF_PI;
+
+ return new double[] {
+ Math.atan2(angle1TanY, angle1TanX),
+ angle2,
+ Geometry.ZERO_PI
+ };
+ }
+
+ final Vector3D crossAxis13 = axis1.crossProduct(axis3);
+
+ final double angle1TanY = vec3.dotProduct(crossAxis13);
+ final double angle1TanX = vec3.dotProduct(axis3);
+
+ final double angle3TanY = invVec1.dotProduct(crossAxis13);
+ final double angle3TanX = invVec1.dotProduct(axis1);
+
+ return new double[] {
+ Math.atan2(angle1TanY, angle1TanX),
+ Math.asin(angle2Sin),
+ Math.atan2(angle3TanY, angle3TanX)
+ };
+ }
+
+ /** Get a sequence of angles around the given Tait-Bryan axes that produce a rotation equivalent
+ * to this instance. The axes are interpreted as being part of an absolute (unmoving) coordinate frame.
+ * @param axis1 first Tait-Bryan axis
+ * @param axis2 second Tait-Bryan axis
+ * @param axis3 third Tait-Bryan axis
+ * @return a sequence of rotation angles around the absolute input axes that produce a rotation equivalent
+ * to this instance
+ */
+ private double[] getAbsoluteTaitBryanAngles(final Vector3D axis1, final Vector3D axis2, final Vector3D axis3) {
+ // A relative axis-angle rotation sequence is equivalent to an absolute one with the rotation
+ // sequence reversed, meaning we can reuse our relative logic here.
+ return reverseArray(getRelativeTaitBryanAngles(axis3, axis2, axis1));
+ }
+
+ /** Get a sequence of angles around the given Euler axes that produce a rotation equivalent
+ * to this instance. The axes are interpreted as being relative to the rotated coordinate frame.
+ * @param axis1 first Euler axis
+ * @param axis2 second Euler axis
+ * @param axis3 third Euler axis
+ * @return a sequence of rotation angles around the relative input axes that produce a rotation equivalent
+ * to this instance
+ */
+ private double[] getRelativeEulerAngles(final Vector3D axis1, final Vector3D axis2, final Vector3D axis3) {
+
+ // Use the same overall approach as with the Tait-Bryan angles: get the first two angles by looking
+ // at the transformed rotation axes and the third by using the inverse.
+
+ final Vector3D crossAxis = axis1.crossProduct(axis2);
+
+ final Vector3D vec1 = apply(axis1);
+ final Vector3D invVec1 = getInverse().apply(axis1);
+
+ final double angle2Cos = vec1.dotProduct(axis1);
+
+ if (angle2Cos < -AXIS_ANGLE_SINGULARITY_THRESHOLD ||
+ angle2Cos > AXIS_ANGLE_SINGULARITY_THRESHOLD) {
+
+ final Vector3D vec2 = apply(axis2);
+
+ final double angle1TanY = vec2.dotProduct(crossAxis);
+ final double angle1TanX = vec2.dotProduct(axis2);
+
+ final double angle2 = angle2Cos > AXIS_ANGLE_SINGULARITY_THRESHOLD ? Geometry.ZERO_PI : Geometry.PI;
+
+ return new double[] {
+ Math.atan2(angle1TanY, angle1TanX),
+ angle2,
+ Geometry.ZERO_PI
+ };
+ }
+
+ final double angle1TanY = vec1.dotProduct(axis2);
+ final double angle1TanX = -vec1.dotProduct(crossAxis);
+
+ final double angle3TanY = invVec1.dotProduct(axis2);
+ final double angle3TanX = invVec1.dotProduct(crossAxis);
+
+ return new double[] {
+ Math.atan2(angle1TanY, angle1TanX),
+ Math.acos(angle2Cos),
+ Math.atan2(angle3TanY, angle3TanX)
+ };
+ }
+
+ /** Get a sequence of angles around the given Euler axes that produce a rotation equivalent
+ * to this instance. The axes are interpreted as being part of an absolute (unmoving) coordinate frame.
+ * @param axis1 first Euler axis
+ * @param axis2 second Euler axis
+ * @param axis3 third Euler axis
+ * @return a sequence of rotation angles around the absolute input axes that produce a rotation equivalent
+ * to this instance
+ */
+ private double[] getAbsoluteEulerAngles(final Vector3D axis1, final Vector3D axis2, final Vector3D axis3) {
+ // A relative axis-angle rotation sequence is equivalent to an absolute one with the rotation
+ // sequence reversed, meaning we can reuse our relative logic here.
+ return reverseArray(getRelativeEulerAngles(axis3, axis2, axis1));
+ }
+
+ /**
+ * Create a new instance from the given quaternion values. The inputs are
+ * normalized.
+ *
+ * @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,
+ * NaN, or infinite
+ */
+ 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);
+ }
+
+ /** 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
+ * normalized.
+ *
+ * @param q quaternion component array
+ * @return new quaterion containing the normalized components
+ * @exception IllegalArgumentException if the array does not have 4 elements
+ */
+ public static QuaternionRotation of(final double[] q) {
+ if (q.length != 4) {
+ throw new IllegalArgumentException("Dimension mismatch: " + q.length + " != 4");
+ }
+ return QuaternionRotation.of(q[0], q[1], q[2], q[3]);
+ }
+
+ /** Return an instance representing a rotation of zero.
+ * @return instance representing a rotation of zero.
+ */
+ public static QuaternionRotation identity() {
+ return IDENTITY_INSTANCE;
+ }
+
+ /** Create a new instance representing a rotation of {@code angle} radians around
+ * {@code axis}.
+ *
+ * <p>
+ * Rotation direction follows the right-hand rule, meaning that if one
+ * places their right hand such that the thumb points in the direction of the vector,
+ * the curl of the fingers indicates the direction of rotation.
+ * </p>
+ *
+ * <p>
+ * Note that the returned quaternion will represent the defined rotation but the values
+ * returned by {@link #getAxis()} and {@link #getAngle()} may not match the ones given here.
+ * This is because the axis and angle are normalized such that the axis has unit length,
+ * and the angle lies in the range {@code [0, pi]}. Depending on the inputs, the axis may
+ * need to be inverted in order for the angle to lie in this range.
+ * </p>
+ *
+ * @param axis the axis of rotation
+ * @param angle angle of rotation in radians
+ * @return a new instance representing the defined rotation
+ *
+ * @throws IllegalNormException if the given axis cannot be normalized
+ * @throws IllegalArgumentException if the angle is NaN or infinite
+ */
+ public static QuaternionRotation fromAxisAngle(final Vector3D axis, final double angle) {
+ // reference formula:
+ // http://www.euclideanspace.com/maths/geometry/rotations/conversions/angleToQuaternion/index.htm
+ final Vector3D normAxis = axis.normalize();
+
+ if (!Double.isFinite(angle)) {
+ throw new IllegalArgumentException("Invalid angle: " + angle);
+ }
+
+ final double halfAngle = 0.5 * angle;
+ final double sinHalfAngle = Math.sin(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 an instance that rotates the first vector to the second.
+ *
+ * <p>Except for a possible scale factor, if the returned instance is
+ * applied to vector {@code u}, it will produce the vector {@code v}. There are an
+ * infinite number of such rotations; this method chooses the one with the smallest
+ * associated angle, meaning the one whose axis is orthogonal to the {@code (u, v)}
+ * plane. If {@code u} and {@code v} are collinear, an arbitrary rotation axis is
+ * chosen.</p>
+ *
+ * @param u origin vector
+ * @param v target vector
+ * @return a new instance that rotates {@code u} to point in the direction of {@code v}
+ * @throws IllegalNormException if either vector has a norm of zero, NaN, or infinity
+ */
+ public static QuaternionRotation createVectorRotation(final Vector3D u, final Vector3D v) {
+
+ double normProduct = Vectors.checkedNorm(u) * Vectors.checkedNorm(v);
+ double dot = u.dotProduct(v);
+
+ if (dot < ANTIPARALLEL_DOT_THRESHOLD * normProduct) {
+ // Special case where u1 = -u2:
+ // create a pi angle rotation around
+ // an arbitrary unit vector orthogonal to u1
+ final Vector3D axis = u.orthogonal();
+
+ return new QuaternionRotation(
+ axis.getX(),
+ axis.getY(),
+ axis.getZ(),
+ 0.0
+ );
+ }
+
+ // General case:
+ // (u1, u2) defines a plane so rotate around the normal of the plane
+
+ // w must equal cos(theta/2); we can calculate this directly using values
+ // we already have with the identity cos(theta/2) = sqrt((1 + cos(theta)) / 2)
+ // and the fact that dot = norm(u1) * norm(u2) * cos(theta).
+ final double w = Math.sqrt(0.5 * (1.0 + (dot / normProduct)));
+
+ // The cross product u1 x u2 must be normalized and then multiplied by
+ // sin(theta/2) in order to set the vectorial part of the quaternion. To
+ // accomplish this, we'll use the following:
+ //
+ // 1) norm(a x b) = norm(a) * norm(b) * sin(theta)
+ // 2) sin(theta/2) = sqrt((1 - cos(theta)) / 2)
+ //
+ // Our full, combined normalization and sine half angle term factor then becomes:
+ //
+ // sqrt((1 - cos(theta)) / 2) / (norm(u1) * norm(u2) * sin(theta))
+ //
+ // This can be simplified to the expression below.
+ final double vectorialScaleFactor = 1.0 / (2.0 * w * normProduct);
+ final Vector3D axis = u.crossProduct(v);
+
+ return new QuaternionRotation(
+ vectorialScaleFactor * axis.getX(),
+ vectorialScaleFactor * axis.getY(),
+ vectorialScaleFactor * axis.getZ(),
+ w
+ );
+ }
+
+ /** Return an instance that rotates the basis defined by the first two vectors into the basis
+ * defined by the second two.
+ *
+ * <p>
+ * The given basis vectors do not have to be directly orthogonal. A right-handed orthonormal
+ * basis is created from each pair by normalizing the first vector, making the second vector
+ * orthogonal to the first, and then taking the cross product. A rotation is then calculated
+ * that rotates the first to the second.
+ * </p>
+ *
+ * @param u1 first vector of the source basis
+ * @param u2 second vector of the source basis
+ * @param v1 first vector of the target basis
+ * @param v2 second vector of the target basis
+ * @return an instance that rotates the source basis to the target basis
+ * @throws IllegalNormException if any of the input vectors cannot be normalized
+ * or the vectors defining either basis are colinear
+ */
+ public static QuaternionRotation createBasisRotation(final Vector3D u1, final Vector3D u2,
+ final Vector3D v1, final Vector3D v2) {
+
+ // calculate orthonormalized bases
+ final Vector3D a = u1.normalize();
+ final Vector3D b = a.orthogonal(u2);
+ final Vector3D c = a.crossProduct(b);
+
+ final Vector3D d = v1.normalize();
+ final Vector3D e = d.orthogonal(v2);
+ final Vector3D f = d.crossProduct(e);
+
+ // create an orthogonal rotation matrix representing the change of basis; this matrix will
+ // be the multiplication of the matrix composed of the column vectors d, e, f and the
+ // inverse of the matrix composed of the column vectors a, b, c (which is simply the transpose since
+ // it's orthogonal).
+ final double m00 = LinearCombination.value(d.getX(), a.getX(), e.getX(), b.getX(), f.getX(), c.getX());
+ final double m01 = LinearCombination.value(d.getX(), a.getY(), e.getX(), b.getY(), f.getX(), c.getY());
+ final double m02 = LinearCombination.value(d.getX(), a.getZ(), e.getX(), b.getZ(), f.getX(), c.getZ());
+
+ final double m10 = LinearCombination.value(d.getY(), a.getX(), e.getY(), b.getX(), f.getY(), c.getX());
+ final double m11 = LinearCombination.value(d.getY(), a.getY(), e.getY(), b.getY(), f.getY(), c.getY());
+ final double m12 = LinearCombination.value(d.getY(), a.getZ(), e.getY(), b.getZ(), f.getY(), c.getZ());
+
+ final double m20 = LinearCombination.value(d.getZ(), a.getX(), e.getZ(), b.getX(), f.getZ(), c.getX());
+ final double m21 = LinearCombination.value(d.getZ(), a.getY(), e.getZ(), b.getY(), f.getZ(), c.getY());
+ final double m22 = LinearCombination.value(d.getZ(), a.getZ(), e.getZ(), b.getZ(), f.getZ(), c.getZ());
+
+
+ return orthogonalRotationMatrixToQuaternion(
+ m00, m01, m02,
+ m10, m11, m12,
+ m20, m21, m22
+ );
+ }
+
+ /** Create a new instance equivalent to the given sequence of axis-angle rotations.
+ * @param sequence the axis-angle rotation sequence to convert to a quaternion rotation
+ * @return instance representing a rotation equivalent to the given axis-angle sequence
+ */
+ public static QuaternionRotation fromAxisAngleSequence(final AxisAngleSequence sequence) {
+ final AxisSequence axes = sequence.getAxisSequence();
+
+ final QuaternionRotation q1 = fromAxisAngle(axes.getAxis1(), sequence.getAngle1());
+ final QuaternionRotation q2 = fromAxisAngle(axes.getAxis2(), sequence.getAngle2());
+ final QuaternionRotation q3 = fromAxisAngle(axes.getAxis3(), sequence.getAngle3());
+
+ if (sequence.getReferenceFrame() == AxisReferenceFrame.ABSOLUTE) {
+ return q3.multiply(q2).multiply(q1);
+ }
+
+ return q1.multiply(q2).multiply(q3);
+ }
+
+ /** Create an instance from an orthogonal rotation matrix.
+ *
+ * @param m00 matrix entry <code>m<sub>0,0</sub></code>
+ * @param m01 matrix entry <code>m<sub>0,1</sub></code>
+ * @param m02 matrix entry <code>m<sub>0,2</sub></code>
+ * @param m10 matrix entry <code>m<sub>1,0</sub></code>
+ * @param m11 matrix entry <code>m<sub>1,1</sub></code>
+ * @param m12 matrix entry <code>m<sub>1,2</sub></code>
+ * @param m20 matrix entry <code>m<sub>2,0</sub></code>
+ * @param m21 matrix entry <code>m<sub>2,1</sub></code>
+ * @param m22 matrix entry <code>m<sub>2,2</sub></code>
+ * @return an instance representing the same 3D rotation as the given matrix
+ */
+ private static QuaternionRotation orthogonalRotationMatrixToQuaternion(
+ final double m00, final double m01, final double m02,
+ final double m10, final double m11, final double m12,
+ final double m20, final double m21, final double m22) {
+
+ // reference formula:
+ // http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/
+
+ // The overall approach here is to take the equations for converting a quaternion to
+ // a matrix along with the fact that 1 = x^2 + y^2 + z^2 + w^2 for a normalized quaternion
+ // and solve for the various terms. This can theoretically be done using just the diagonal
+ // terms from the matrix. However, there are a few issues with this:
+ // 1) The term that we end up taking the square root of may be negative.
+ // 2) It's ambiguous as to whether we should use a plus or minus for the value of the
+ // square root.
+ // We'll address these concerns by only calculating a single term from one of the diagonal
+ // elements and then calculate the rest from the non-diagonals, which do not involve
+ // a square root. This solves the first issue since we can make sure to choose a diagonal
+ // element that will not cause us to take a square root of a negative number. The second
+ // issue is solved since only the relative signs between the quaternion terms are important
+ // (q and -q represent the same 3D rotation). It therefore doesn't matter whether we choose
+ // a plus or minus for our initial square root solution.
+
+ final double trace = m00 + m11 + m22;
+
+ double w;
+ double x;
+ double y;
+ double z;
+
+ if (trace > 0.0) {
+ // let s = 4*w
+ final double s = 2.0 * Math.sqrt(1.0 + trace);
+ final double sinv = 1.0 / s;
+
+ x = (m21 - m12) * sinv;
+ y = (m02 - m20) * sinv;
+ z = (m10 - m01) * sinv;
+ w = 0.25 * s;
+ }
+ else if ((m00 > m11) && (m00 > m22)) {
+ // let s = 4*x
+ final double s = 2.0 * Math.sqrt(1.0 + m00 - m11 - m22);
+ final double sinv = 1.0 / s;
+
+ x = 0.25 * s;
+ y = (m01 + m10) * sinv;
+ z = (m02 + m20) * sinv;
+ w = (m21 - m12) * sinv;
+ }
+ else if (m11 > m22) {
+ // let s = 4*y
+ final double s = 2.0 * Math.sqrt(1.0 + m11 - m00 - m22);
+ final double sinv = 1.0 / s;
+
+ x = (m01 + m10) * sinv;
+ y = 0.25 * s;
+ z = (m21 + m12) * sinv;
+ w = (m02 - m20) * sinv;
+ }
+ else {
+ // let s = 4*z
+ final double s = 2.0 * Math.sqrt(1.0 + m22 - m00 - m11);
+ final double sinv = 1.0 / s;
+
+ x = (m02 + m20) * sinv;
+ y = (m21 + m12) * sinv;
+ z = 0.25 * s;
+ w = (m10 - m01) * sinv;
+ }
+
+ return new QuaternionRotation(x, y, z, w);
+ }
+
+ /** Reverse the elements in {@code arr}. The array is returned.
+ * @param arr the array to reverse
+ * @return the input array with the elements reversed
+ */
+ private static double[] reverseArray(final double[] arr) {
+
+ final int len = arr.length;
+ double temp;
+
+ int i;
+ int j;
+
+ for (i=0, j=len-1; i < len / 2; ++i, --j) {
+ temp = arr[i];
+ arr[i] = arr[j];
+ arr[j] = temp;
+ }
+
+ return arr;
+ }
+}
diff --git a/commons-geometry-euclidean/src/main/java/org/apache/commons/geometry/euclidean/threed/rotation/package-info.java b/commons-geometry-euclidean/src/main/java/org/apache/commons/geometry/euclidean/threed/rotation/package-info.java
new file mode 100644
index 0000000..f1ad025
--- /dev/null
+++ b/commons-geometry-euclidean/src/main/java/org/apache/commons/geometry/euclidean/threed/rotation/package-info.java
@@ -0,0 +1,47 @@
+/*
+ * 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.
+ */
+/**
+ *
+ * <p>
+ * This package provides components related to rotations in 3 dimensional
+ * Euclidean space.
+ * </p>
+ *
+ * <h2>Conventions</h2>
+ * <p>
+ * There are numerous conventions that must be decided when attempting to
+ * define 3-dimensional rotations. The following list contains some of the
+ * primary rotation conventions for this package. All method parameters, return
+ * values, and operations follow these conventions unless explicitly stated otherwise.
+ *
+ * <ul>
+ * <li><strong>Active</strong> -- All rotations are "active", meaning that
+ * they transform the vector or point they are applied to instead of transforming the
+ * coordinate system. An active rotation can be converted to a passive one and vice
+ * versa simply by taking the inverse. See
+ * <a href="https://en.wikipedia.org/wiki/Active_and_passive_transformation">here</a> for more details.
+ * </li>
+ * <li><strong>Right-handed</strong> -- All rotation directions follow the
+ * <a href="https://en.wikipedia.org/wiki/Right-hand_rule">right hand rule</a>.
+ * This means that the direction of rotation for a given axis and angle is the same
+ * direction that one's fingers curl if the thumb is pointed along the axis of rotation.
+ * </li>
+ * <li><strong>Radians</strong> -- All angles are in radians.
+ * </li>
+ * </ul>
+ */
+package org.apache.commons.geometry.euclidean.threed.rotation;
diff --git a/commons-geometry-euclidean/src/main/java/org/apache/commons/geometry/euclidean/twod/AffineTransformMatrix2D.java b/commons-geometry-euclidean/src/main/java/org/apache/commons/geometry/euclidean/twod/AffineTransformMatrix2D.java
new file mode 100644
index 0000000..4cfcaee
--- /dev/null
+++ b/commons-geometry-euclidean/src/main/java/org/apache/commons/geometry/euclidean/twod/AffineTransformMatrix2D.java
@@ -0,0 +1,552 @@
+/*
+ * 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.geometry.euclidean.twod;
+
+import java.io.Serializable;
+
+import org.apache.commons.geometry.core.exception.IllegalNormException;
+import org.apache.commons.geometry.core.internal.DoubleFunction2N;
+import org.apache.commons.geometry.euclidean.exception.NonInvertibleTransformException;
+import org.apache.commons.geometry.euclidean.internal.Matrices;
+import org.apache.commons.geometry.euclidean.internal.Vectors;
+import org.apache.commons.numbers.arrays.LinearCombination;
+import org.apache.commons.numbers.core.Precision;
+
+/** Class using a matrix to represent affine transformations in 2 dimensional Euclidean space.
+*
+* <p>Instances of this class use a 3x3 matrix for all transform operations.
+* The last row of this matrix is always set to the values <code>[0 0 1]</code> and so
+* is not stored. Hence, the methods in this class that accept or return arrays always
+* use arrays containing 6 elements, instead of 9.
+* </p>
+*/
+public final class AffineTransformMatrix2D implements Serializable {
+
+ /** Serializable version identifier */
+ private static final long serialVersionUID = 20181005L;
+
+ /** The number of internal matrix elements */
+ private static final int NUM_ELEMENTS = 6;
+
+ /** String used to start the transform matrix string representation */
+ private static final String MATRIX_START = "[ ";
+
+ /** String used to end the transform matrix string representation */
+ private static final String MATRIX_END = " ]";
+
+ /** String used to separate elements in the matrix string representation */
+ private static final String ELEMENT_SEPARATOR = ", ";
+
+ /** String used to separate rows in the matrix string representation */
+ private static final String ROW_SEPARATOR = "; ";
+
+ /** Shared transform set to the identity matrix. */
+ private static final AffineTransformMatrix2D IDENTITY_INSTANCE = new AffineTransformMatrix2D(
+ 1, 0, 0,
+ 0, 1, 0
+ );
+
+ /** Transform matrix entry <code>m<sub>0,0</sub></code> */
+ private final double m00;
+ /** Transform matrix entry <code>m<sub>0,1</sub></code> */
+ private final double m01;
+ /** Transform matrix entry <code>m<sub>0,2</sub></code> */
+ private final double m02;
+
+ /** Transform matrix entry <code>m<sub>1,0</sub></code> */
+ private final double m10;
+ /** Transform matrix entry <code>m<sub>1,1</sub></code> */
+ private final double m11;
+ /** Transform matrix entry <code>m<sub>1,2</sub></code> */
+ private final double m12;
+
+ /**
+ * Simple constructor; sets all internal matrix elements.
+ * @param m00 matrix entry <code>m<sub>0,0</sub></code>
+ * @param m01 matrix entry <code>m<sub>0,1</sub></code>
+ * @param m02 matrix entry <code>m<sub>0,2</sub></code>
+ * @param m10 matrix entry <code>m<sub>1,0</sub></code>
+ * @param m11 matrix entry <code>m<sub>1,1</sub></code>
+ * @param m12 matrix entry <code>m<sub>1,2</sub></code>
+ */
+ private AffineTransformMatrix2D(
+ final double m00, final double m01, final double m02,
+ final double m10, final double m11, final double m12) {
+
+ this.m00 = m00;
+ this.m01 = m01;
+ this.m02 = m02;
+
+ this.m10 = m10;
+ this.m11 = m11;
+ this.m12 = m12;
+ }
+
+ /** Return a 6 element array containing the variable elements from the
+ * internal transformation matrix. The elements are in row-major order.
+ * The array indices map to the internal matrix as follows:
+ * <pre>
+ * [
+ * arr[0], arr[1], arr[2],
+ * arr[3], arr[4], arr[5],
+ * 0 0 1
+ * ]
+ * </pre>
+ * @return 6 element array containing the variable elements from the
+ * internal transformation matrix
+ */
+ public double[] toArray() {
+ return new double[] {
+ m00, m01, m02,
+ m10, m11, m12
+ };
+ }
+
+ /** Apply this transform to the given vector, returning the result as a new instance.
+ *
+ * <p>The transformed vector is computed by creating a 3-element column vector from the
+ * coordinates in the input and setting the last element to 1. This is then multiplied with the
+ * 3x3 transform matrix to produce the transformed vector. The {@code 1} in the last position
+ * is ignored.
+ * <pre>
+ * [ m00 m01 m02 ] [ x ] [ x']
+ * [ m10 m11 m12 ] * [ y ] = [ y']
+ * [ 0 0 1 ] [ 1 ] [ 1 ]
+ * </pre>
+ *
+ * @param vec the vector to transform
+ * @return the new, transformed vector
+ */
+ public Vector2D apply(final Vector2D vec) {
+ final double x = vec.getX();
+ final double y = vec.getY();
+
+ final double resultX = LinearCombination.value(m00, x, m01, y) + m02;
+ final double resultY = LinearCombination.value(m10, x, m11, y) + m12;
+
+ return Vector2D.of(resultX, resultY);
+ }
+
+ /** Apply this transform to the given vector, ignoring translations.
+ *
+ * <p>This method can be used to transform vector instances representing displacements between points.
+ * For example, if {@code v} represents the difference between points {@code p1} and {@code p2},
+ * then {@code transform.applyVector(v)} will represent the difference between {@code p1} and {@code p2}
+ * after {@code transform} is applied.
+ * </p>
+ *
+ * <p>The transformed vector is computed by creating a 3-element column vector from the
+ * coordinates in the input and setting the last element to 0. This is then multiplied with the
+ * 3x3 transform matrix to produce the transformed vector. The {@code 0} in the last position
+ * is ignored.
+ * <pre>
+ * [ m00 m01 m02 ] [ x ] [ x']
+ * [ m10 m11 m12 ] * [ y ] = [ y']
+ * [ 0 0 1 ] [ 0 ] [ 0 ]
+ * </pre>
+ *
+ * @param vec the vector to transform
+ * @return the new, transformed vector
+ * @see #applyDirection(Vector2D)
+ */
+ public Vector2D applyVector(final Vector2D vec) {
+ return applyVector(vec, Vector2D::of);
+ }
+
+ /** Apply this transform to the given vector, ignoring translations and normalizing the
+ * result. This is equivalent to {@code transform.applyVector(vec).normalize()} but without
+ * the intermediate vector instance.
+ * @param vec the vector to transform
+ * @return the new, transformed unit vector
+ * @throws IllegalNormException if the transformed vector coordinates cannot be normalized
+ * @see #applyVector(Vector2D)
+ */
+ public Vector2D applyDirection(final Vector2D vec) {
+ return applyVector(vec, Vector2D::normalize);
+ }
+
+ /** Apply a translation to the current instance, returning the result as a new transform.
+ * @param translation vector containing the translation values for each axis
+ * @return a new transform containing the result of applying a translation to
+ * the current instance
+ */
+ public AffineTransformMatrix2D translate(final Vector2D translation) {
+ return translate(translation.getX(), translation.getY());
+ }
+
+ /** Apply a translation to the current instance, returning the result as a new transform.
+ * @param x translation in the x direction
+ * @param y translation in the y direction
+ * @return a new transform containing the result of applying a translation to
+ * the current instance
+ */
+ public AffineTransformMatrix2D translate(final double x, final double y) {
+ return new AffineTransformMatrix2D(
+ m00, m01, m02 + x,
+ m10, m11, m12 + y
+ );
+ }
+
+ /** Apply a scale operation to the current instance, returning the result as a new transform.
+ * @param factor the scale factor to apply to all axes
+ * @return a new transform containing the result of applying a scale operation to
+ * the current instance
+ */
+ public AffineTransformMatrix2D scale(final double factor) {
+ return scale(factor, factor);
+ }
+
+ /** Apply a scale operation to the current instance, returning the result as a new transform.
+ * @param scaleFactors vector containing scale factors for each axis
+ * @return a new transform containing the result of applying a scale operation to
+ * the current instance
+ */
+ public AffineTransformMatrix2D scale(final Vector2D scaleFactors) {
+ return scale(scaleFactors.getX(), scaleFactors.getY());
+ }
+
+ /** Apply a scale operation to the current instance, returning the result as a new transform.
+ * @param x scale factor for the x axis
+ * @param y scale factor for the y axis
+ * @return a new transform containing the result of applying a scale operation to
+ * the current instance
+ */
+ public AffineTransformMatrix2D scale(final double x, final double y) {
+ return new AffineTransformMatrix2D(
+ m00 * x, m01 * x, m02 * x,
+ m10 * y, m11 * y, m12 * y
+ );
+ }
+
+ /** Apply a <em>counterclockwise</em> rotation to the current instance, returning the result as a
+ * new transform.
+ * @param angle the angle of counterclockwise rotation in radians
+ * @return a new transform containing the result of applying a rotation to the
+ * current instance
+ */
+ public AffineTransformMatrix2D rotate(final double angle) {
+ return multiply(createRotation(angle), this);
+ }
+
+ /** Apply a <em>counterclockwise</em> rotation about the given center point to the current instance,
+ * returning the result as a new transform. This is accomplished by translating the center to the origin,
+ * applying the rotation, and then translating back.
+ * @param center the center of rotation
+ * @param angle the angle of counterclockwise rotation in radians
+ * @return a new transform containing the result of applying a rotation about the given
+ * center point to the current instance
+ */
+ public AffineTransformMatrix2D rotate(final Vector2D center, final double angle) {
+ return multiply(createRotation(center, angle), this);
+ }
+
+ /** Get a new transform created by multiplying this instance by the argument.
+ * This is equivalent to the expression {@code A * M} where {@code A} is the
+ * current transform matrix and {@code M} is the given transform matrix. In
+ * terms of transformations, applying the returned matrix is equivalent to
+ * applying {@code M} and <em>then</em> applying {@code A}. In other words,
+ * the rightmost transform is applied first.
+ *
+ * @param m the transform to multiply with
+ * @return the result of multiplying the current instance by the given
+ * transform matrix
+ */
+ public AffineTransformMatrix2D multiply(final AffineTransformMatrix2D m) {
+ return multiply(this, m);
+ }
+
+ /** Get a new transform created by multiplying the argument by this instance.
+ * This is equivalent to the expression {@code M * A} where {@code A} is the
+ * current transform matrix and {@code M} is the given transform matrix. In
+ * terms of transformations, applying the returned matrix is equivalent to
+ * applying {@code A} and <em>then</em> applying {@code M}. In other words,
+ * the rightmost transform is applied first.
+ *
+ * @param m the transform to multiply with
+ * @return the result of multiplying the given transform matrix by the current
+ * instance
+ */
+ public AffineTransformMatrix2D premultiply(final AffineTransformMatrix2D m) {
+ return multiply(m, this);
+ }
+
+ /** Get a new transform representing the inverse of the current instance.
+ * @return inverse transform
+ * @throws NonInvertibleTransformException if the transform matrix cannot be inverted
+ */
+ public AffineTransformMatrix2D getInverse() {
+
+ // Our full matrix is 3x3 but we can significantly reduce the amount of computations
+ // needed here since we know that our last row is [0 0 1].
+
+ // compute the determinant of the matrix
+ final double det = Matrices.determinant(
+ m00, m01,
+ m10, m11
+ );
+
+ if (!Vectors.isRealNonZero(det)) {
+ throw new NonInvertibleTransformException("Transform is not invertible; matrix determinant is " + det);
+ }
+
+ // validate the remaining matrix elements that were not part of the determinant
+ validateElementForInverse(m02);
+ validateElementForInverse(m12);
+
+ // compute the necessary elements of the cofactor matrix
+ // (we need all but the last column)
+
+ final double invDet = 1.0 / det;
+
+ final double c00 = invDet * m11;
+ final double c01 = - invDet * m10;
+
+ final double c10 = - invDet * m01;
+ final double c11 = invDet * m00;
+
+ final double c20 = invDet * Matrices.determinant(m01, m02, m11, m12);
+ final double c21 = - invDet * Matrices.determinant(m00, m02, m10, m12);
+
+ return new AffineTransformMatrix2D(
+ c00, c10, c20,
+ c01, c11, c21
+ );
+ }
+
+ /** {@inheritDoc} */
+ @Override
+ public int hashCode() {
+ final int prime = 31;
+ int result = 1;
+
+ result = (result * prime) + (Double.hashCode(m00) - Double.hashCode(m01) + Double.hashCode(m02));
+ result = (result * prime) + (Double.hashCode(m10) - Double.hashCode(m11) + Double.hashCode(m12));
+
+ return result;
+ }
+
+ /**
+ * Return true if the given object is an instance of {@link AffineTransformMatrix2D}
+ * and all matrix element values are exactly equal.
+ * @param obj object to test for equality with the current instance
+ * @return true if all transform matrix elements are exactly equal; otherwise false
+ */
+ @Override
+ public boolean equals(Object obj) {
+ if (this == obj) {
+ return true;
+ }
+ if (!(obj instanceof AffineTransformMatrix2D)) {
+ return false;
+ }
+
+ final AffineTransformMatrix2D other = (AffineTransformMatrix2D) obj;
+
+ return Precision.equals(this.m00, other.m00) &&
+ Precision.equals(this.m01, other.m01) &&
+ Precision.equals(this.m02, other.m02) &&
+
+ Precision.equals(this.m10, other.m10) &&
+ Precision.equals(this.m11, other.m11) &&
+ Precision.equals(this.m12, other.m12);
+ }
+
+ /** {@inheritDoc} */
+ @Override
+ public String toString() {
+ final StringBuilder sb = new StringBuilder();
+
+ sb.append(MATRIX_START)
+
+ .append(m00)
+ .append(ELEMENT_SEPARATOR)
+ .append(m01)
+ .append(ELEMENT_SEPARATOR)
+ .append(m02)
+ .append(ROW_SEPARATOR)
+
+ .append(m10)
+ .append(ELEMENT_SEPARATOR)
+ .append(m11)
+ .append(ELEMENT_SEPARATOR)
+ .append(m12)
+
+ .append(MATRIX_END);
+
+ return sb.toString();
+ }
+
+ /** Multiplies the given vector by the 2x2 linear transformation matrix contained in the
+ * upper-right corner of the affine transformation matrix. This applies all transformation
+ * operations except for translations. The computed coordinates are passed to the given
+ * factory function.
+ * @param <T> factory output type
+ * @param vec the vector to transform
+ * @param factory the factory instance that will be passed the transformed coordinates
+ * @return the factory return value
+ */
+ private <T> T applyVector(final Vector2D vec, final DoubleFunction2N<T> factory) {
+ final double x = vec.getX();
+ final double y = vec.getY();
+
+ final double resultX = LinearCombination.value(m00, x, m01, y);
+ final double resultY = LinearCombination.value(m10, x, m11, y);
+
+ return factory.apply(resultX, resultY);
+ }
+
+ /** Get a new transform with the given matrix elements. The array must contain 6 elements.
+ * @param arr 6-element array containing values for the variable entries in the
+ * transform matrix
+ * @return a new transform initialized with the given matrix values
+ * @throws IllegalArgumentException if the array does not have 6 elements
+ */
+ public static AffineTransformMatrix2D of(final double ... arr) {
+ if (arr.length != NUM_ELEMENTS) {
+ throw new IllegalArgumentException("Dimension mismatch: " + arr.length + " != " + NUM_ELEMENTS);
+ }
+
+ return new AffineTransformMatrix2D(
+ arr[0], arr[1], arr[2],
+ arr[3], arr[4], arr[5]
+ );
+ }
+
+ /** Get the transform representing the identity matrix. This transform does not
+ * modify point or vector values when applied.
+ * @return transform representing the identity matrix
+ */
+ public static AffineTransformMatrix2D identity() {
+ return IDENTITY_INSTANCE;
+ }
+
+ /** Create a transform representing the given translation.
+ * @param translation vector containing translation values for each axis
+ * @return a new transform representing the given translation
+ */
+ public static AffineTransformMatrix2D createTranslation(final Vector2D translation) {
+ return createTranslation(translation.getX(), translation.getY());
+ }
+
+ /** Create a transform representing the given translation.
+ * @param x translation in the x direction
+ * @param y translation in the y direction
+ * @return a new transform representing the given translation
+ */
+ public static AffineTransformMatrix2D createTranslation(final double x, final double y) {
+ return new AffineTransformMatrix2D(
+ 1, 0, x,
+ 0, 1, y
+ );
+ }
+
+ /** Create a transform representing a scale operation with the given scale factor applied to all axes.
+ * @param factor scale factor to apply to all axes
+ * @return a new transform representing a uniform scaling in all axes
+ */
+ public static AffineTransformMatrix2D createScale(final double factor) {
+ return createScale(factor, factor);
+ }
+
+ /** Create a transform representing a scale operation.
+ * @param factors vector containing scale factors for each axis
+ * @return a new transform representing a scale operation
+ */
+ public static AffineTransformMatrix2D createScale(final Vector2D factors) {
+ return createScale(factors.getX(), factors.getY());
+ }
+
+ /** Create a transform representing a scale operation.
+ * @param x scale factor for the x axis
+ * @param y scale factor for the y axis
+ * @return a new transform representing a scale operation
+ */
+ public static AffineTransformMatrix2D createScale(final double x, final double y) {
+ return new AffineTransformMatrix2D(
+ x, 0, 0,
+ 0, y, 0
+ );
+ }
+
+ /** Create a transform representing a <em>counterclockwise</em> rotation of {@code angle}
+ * radians around the origin.
+ * @param angle the angle of rotation in radians
+ * @return a new transform representing the rotation
+ */
+ public static AffineTransformMatrix2D createRotation(final double angle) {
+ final double sin = Math.sin(angle);
+ final double cos = Math.cos(angle);
+
+ return new AffineTransformMatrix2D(
+ cos, -sin, 0,
+ sin, cos, 0
+ );
+ }
+
+ /** Create a transform representing a <em>counterclockwise</em> rotation of {@code angle}
+ * radians around the given center point. This is accomplished by translating the center point
+ * to the origin, applying the rotation, and then translating back.
+ * @param center the center of rotation
+ * @param angle the angle of rotation in radians
+ * @return a new transform representing the rotation about the given center
+ */
+ public static AffineTransformMatrix2D createRotation(final Vector2D center, final double angle) {
+ final double x = center.getX();
+ final double y = center.getY();
+
+ final double sin = Math.sin(angle);
+ final double cos = Math.cos(angle);
+
+ return new AffineTransformMatrix2D(
+ cos, -sin, (-x * cos) + (y * sin) + x,
+ sin, cos, (-x * sin) - (y * cos) + y
+ );
+ }
+
+ /** Multiply two transform matrices together.
+ * @param a first transform
+ * @param b second transform
+ * @return the transform computed as {@code a x b}
+ */
+ private static AffineTransformMatrix2D multiply(final AffineTransformMatrix2D a, final AffineTransformMatrix2D b) {
+
+ final double c00 = LinearCombination.value(a.m00, b.m00, a.m01, b.m10);
+ final double c01 = LinearCombination.value(a.m00, b.m01, a.m01, b.m11);
+ final double c02 = LinearCombination.value(a.m00, b.m02, a.m01, b.m12) + a.m02;
+
+ final double c10 = LinearCombination.value(a.m10, b.m00, a.m11, b.m10);
+ final double c11 = LinearCombination.value(a.m10, b.m01, a.m11, b.m11);
+ final double c12 = LinearCombination.value(a.m10, b.m02, a.m11, b.m12) + a.m12;
+
+ return new AffineTransformMatrix2D(
+ c00, c01, c02,
+ c10, c11, c12
+ );
+ }
+
+ /** Checks that the given matrix element is valid for use in calculation of
+ * a matrix inverse. Throws a {@link NonInvertibleTransformException} if not.
+ * @param element matrix entry to check
+ * @throws NonInvertibleTransformException if the element is not valid for use
+ * in calculating a matrix inverse, ie if it is NaN or infinite.
+ */
+ private static void validateElementForInverse(final double element) {
+ if (!Double.isFinite(element)) {
+ throw new NonInvertibleTransformException("Transform is not invertible; invalid matrix element: " + element);
+ }
+ }
+}
diff --git a/commons-geometry-euclidean/src/main/java/org/apache/commons/geometry/euclidean/twod/Vector2D.java b/commons-geometry-euclidean/src/main/java/org/apache/commons/geometry/euclidean/twod/Vector2D.java
index ba1cd64..2fa8784 100644
--- a/commons-geometry-euclidean/src/main/java/org/apache/commons/geometry/euclidean/twod/Vector2D.java
+++ b/commons-geometry-euclidean/src/main/java/org/apache/commons/geometry/euclidean/twod/Vector2D.java
@@ -309,6 +309,16 @@ public class Vector2D extends MultiDimensionalEuclideanVector<Vector2D> {
return LinearCombination.value(x1, y1, -x2, y2);
}
+ /** Apply the given transform to this vector, returning the result as a
+ * new vector instance.
+ * @param transform the transform to apply
+ * @return a new, transformed vector
+ * @see AffineTransformMatrix2D#apply(Vector2D)
+ */
+ public Vector2D transform(AffineTransformMatrix2D transform) {
+ return transform.apply(this);
+ }
+
/**
* Get a hashCode for the 2D coordinates.
* <p>
diff --git a/commons-geometry-euclidean/src/test/java/org/apache/commons/geometry/euclidean/EuclideanTestUtils.java b/commons-geometry-euclidean/src/test/java/org/apache/commons/geometry/euclidean/EuclideanTestUtils.java
index 959a75f..1225931 100644
--- a/commons-geometry-euclidean/src/test/java/org/apache/commons/geometry/euclidean/EuclideanTestUtils.java
+++ b/commons-geometry-euclidean/src/test/java/org/apache/commons/geometry/euclidean/EuclideanTestUtils.java
@@ -41,12 +41,115 @@ import org.apache.commons.geometry.euclidean.twod.SubLine;
import org.apache.commons.geometry.euclidean.twod.Vector2D;
import org.junit.Assert;
-/** Class containing various euclidean-related test utilities.
+/**
+ * Class containing various euclidean-related test utilities.
*/
public class EuclideanTestUtils {
- /** Asserts that corresponding values in the given vectors are equal, using the specified
- * tolerance value.
+ /** Callback interface for {@link #permute(double, double, double, PermuteCallback2D)}. */
+ @FunctionalInterface
+ public static interface PermuteCallback2D {
+ void accept(double x, double y);
+ }
+
+ /** Callback interface for {@link #permute(double, double, double, PermuteCallback3D)} */
+ @FunctionalInterface
+ public static interface PermuteCallback3D {
+ void accept(double x, double y, double z);
+ }
+
+ /** Iterate through all {@code (x, y)} permutations for the given range of numbers and
+ * call {@code callback} for each.
+ *
+ * @param min the minimum number in the range
+ * @param max the maximum number in the range
+ * @param step the step (increment) value for the range
+ * @param callback callback to invoke for each permutation.
+ */
+ public static void permute(double min, double max, double step, PermuteCallback2D callback) {
+ permuteInternal(min, max, step, false, callback);
+ }
+
+ /** Same as {@link #permute(double, double, double, PermuteCallback2D)} but skips the {@code (0, 0))}
+ * permutation.
+ *
+ * @param min the minimum number in the range
+ * @param max the maximum number in the range
+ * @param step the step (increment) value for the range
+ * @param callback callback to invoke for each permutation.
+ */
+ public static void permuteSkipZero(double min, double max, double step, PermuteCallback2D callback) {
+ permuteInternal(min, max, step, true, callback);
+ }
+
+ /** Internal permutation method. Iterates through all {@code (x, y)} permutations for the given range
+ * of numbers and calls {@code callback} for each.
+ *
+ * @param min the minimum number in the range
+ * @param max the maximum number in the range
+ * @param step the step (increment) value for the range
+ * @param skipZero if true, the {@code (0, 0)} permutation will be skipped
+ * @param callback callback to invoke for each permutation.
+ */
+ private static void permuteInternal(double min, double max, double step, boolean skipZero, PermuteCallback2D callback) {
+ for (double x = min; x <= max; x += step) {
+ for (double y = min; y <= max; y += step) {
+ if (!skipZero || (x != 0.0 || y != 0.0)) {
+ callback.accept(x, y);
+ }
+ }
+ }
+ }
+
+ /** Iterate through all {@code (x, y, z)} permutations for the given range of numbers and
+ * call {@code callback} for each.
+ *
+ * @param min the minimum number in the range
+ * @param max the maximum number in the range
+ * @param step the step (increment) value for the range
+ * @param callback callback to invoke for each permutation.
+ */
+ public static void permute(double min, double max, double step, PermuteCallback3D callback) {
+ permuteInternal(min, max, step, false, callback);
+ }
+
+ /** Same as {@link #permute(double, double, double, PermuteCallback3D)} but skips the {@code (0, 0, 0)}
+ * permutation.
+ *
+ * @param min the minimum number in the range
+ * @param max the maximum number in the range
+ * @param step the step (increment) value for the range
+ * @param callback callback to invoke for each permutation.
+ */
+ public static void permuteSkipZero(double min, double max, double step, PermuteCallback3D callback) {
+ permuteInternal(min, max, step, true, callback);
+ }
+
+ /** Internal permutation method. Iterates through all {@code (x, y)} permutations for the given range
+ * of numbers and calls {@code callback} for each.
+ *
+ * @param min the minimum number in the range
+ * @param max the maximum number in the range
+ * @param step the step (increment) value for the range
+ * @param skipZero if true, the {@code (0, 0, 0)} permutation will be skipped
+ * @param callback callback to invoke for each permutation.
+ */
+ private static void permuteInternal(double min, double max, double step, boolean skipZero, PermuteCallback3D callback) {
+ for (double x = min; x <= max; x += step) {
+ for (double y = min; y <= max; y += step) {
+ for (double z = min; z <= max; z += step) {
+ if (!skipZero || (x != 0.0 || y != 0.0 || z != 0.0)) {
+ callback.accept(x, y, z);
+ }
+ }
+ }
+ }
+ }
+
+ /**
+ * Asserts that corresponding values in the given vectors are equal, using the
+ * specified tolerance value.
+ *
* @param expected
* @param actual
* @param tolerance
@@ -56,8 +159,10 @@ public class EuclideanTestUtils {
Assert.assertEquals(msg, expected.getX(), actual.getX(), tolerance);
}
- /** Asserts that corresponding values in the given vectors are equal, using the specified
- * tolerance value.
+ /**
+ * Asserts that corresponding values in the given vectors are equal, using the
+ * specified tolerance value.
+ *
* @param expected
* @param actual
* @param tolerance
@@ -68,8 +173,10 @@ public class EuclideanTestUtils {
Assert.assertEquals(msg, expected.getY(), actual.getY(), tolerance);
}
- /** Asserts that corresponding values in the given vectors are equal, using the specified
- * tolerance value.
+ /**
+ * Asserts that corresponding values in the given vectors are equal, using the
+ * specified tolerance value.
+ *
* @param expected
* @param actual
* @param tolerance
@@ -81,7 +188,9 @@ public class EuclideanTestUtils {
Assert.assertEquals(msg, expected.getZ(), actual.getZ(), tolerance);
}
- /** Asserts that the given value is positive infinity.
+ /**
+ * Asserts that the given value is positive infinity.
+ *
* @param value
*/
public static void assertPositiveInfinity(double value) {
@@ -90,7 +199,9 @@ public class EuclideanTestUtils {
Assert.assertTrue(msg, value > 0);
}
- /** Asserts that the given value is negative infinity..
+ /**
+ * Asserts that the given value is negative infinity..
+ *
* @param value
*/
public static void assertNegativeInfinity(double value) {
@@ -99,7 +210,9 @@ public class EuclideanTestUtils {
Assert.assertTrue(msg, value < 0);
}
- /** Get a string representation of an {@link IntervalsSet}.
+ /**
+ * Get a string representation of an {@link IntervalsSet}.
+ *
* @param intervalsSet region to dump
* @return string representation of the region
*/
@@ -110,8 +223,7 @@ public class EuclideanTestUtils {
@Override
protected void formatHyperplane(final Hyperplane<Vector1D> hyperplane) {
final OrientedPoint h = (OrientedPoint) hyperplane;
- getFormatter().format("%22.15e %b %22.15e",
- h.getLocation().getX(), h.isDirect(), h.getTolerance());
+ getFormatter().format("%22.15e %b %22.15e", h.getLocation().getX(), h.isDirect(), h.getTolerance());
}
};
@@ -119,7 +231,9 @@ public class EuclideanTestUtils {
return visitor.getDump();
}
- /** Get a string representation of a {@link PolygonsSet}.
+ /**
+ * Get a string representation of a {@link PolygonsSet}.
+ *
* @param polygonsSet region to dump
* @return string representation of the region
*/
@@ -140,7 +254,9 @@ public class EuclideanTestUtils {
return visitor.getDump();
}
- /** Get a string representation of a {@link PolyhedronsSet}.
+ /**
+ * Get a string representation of a {@link PolyhedronsSet}.
+ *
* @param polyhedronsSet region to dump
* @return string representation of the region
*/
@@ -163,10 +279,12 @@ public class EuclideanTestUtils {
return visitor.getDump();
}
- /** Parse a string representation of an {@link IntervalsSet}.
+ /**
+ * Parse a string representation of an {@link IntervalsSet}.
+ *
* @param s string to parse
* @return parsed region
- * @exception IOException if the string cannot be read
+ * @exception IOException if the string cannot be read
* @exception ParseException if the string cannot be parsed
*/
public static IntervalsSet parseIntervalsSet(final String s)
@@ -184,10 +302,12 @@ public class EuclideanTestUtils {
return new IntervalsSet(builder.getTree(), builder.getTolerance());
}
- /** Parse a string representation of a {@link PolygonsSet}.
+ /**
+ * Parse a string representation of a {@link PolygonsSet}.
+ *
* @param s string to parse
* @return parsed region
- * @exception IOException if the string cannot be read
+ * @exception IOException if the string cannot be read
* @exception ParseException if the string cannot be parsed
*/
public static PolygonsSet parsePolygonsSet(final String s)
@@ -205,10 +325,12 @@ public class EuclideanTestUtils {
return new PolygonsSet(builder.getTree(), builder.getTolerance());
}
- /** Parse a string representation of a {@link PolyhedronsSet}.
+ /**
+ * Parse a string representation of a {@link PolyhedronsSet}.
+ *
* @param s string to parse
* @return parsed region
- * @exception IOException if the string cannot be read
+ * @exception IOException if the string cannot be read
* @exception ParseException if the string cannot be parsed
*/
public static PolyhedronsSet parsePolyhedronsSet(final String s)
@@ -228,10 +350,10 @@ public class EuclideanTestUtils {
return new PolyhedronsSet(builder.getTree(), builder.getTolerance());
}
-
-
- /** Prints a string representation of the given 1D {@link BSPTree} to
- * the console. This is intended for quick debugging of small trees.
+ /**
+ * Prints a string representation of the given 1D {@link BSPTree} to the
+ * console. This is intended for quick debugging of small trees.
+ *
* @param tree
*/
public static void printTree1D(BSPTree<Vector1D> tree) {
@@ -239,8 +361,10 @@ public class EuclideanTestUtils {
System.out.println(printer.writeAsString(tree));
}
- /** Prints a string representation of the given 2D {@link BSPTree} to
- * the console. This is intended for quick debugging of small trees.
+ /**
+ * Prints a string representation of the given 2D {@link BSPTree} to the
+ * console. This is intended for quick debugging of small trees.
+ *
* @param tree
*/
public static void printTree2D(BSPTree<Vector2D> tree) {
@@ -248,8 +372,10 @@ public class EuclideanTestUtils {
System.out.println(printer.writeAsString(tree));
}
- /** Prints a string representation of the given 3D {@link BSPTree} to
- * the console. This is intended for quick debugging of small trees.
+ /**
+ * Prints a string representation of the given 3D {@link BSPTree} to the
+ * console. This is intended for quick debugging of small trees.
+ *
* @param tree
*/
public static void printTree3D(BSPTree<Vector3D> tree) {
@@ -257,8 +383,8 @@ public class EuclideanTestUtils {
System.out.println(printer.writeAsString(tree));
}
-
- /** Class for creating string representations of 1D {@link BSPTree}s.
+ /**
+ * Class for creating string representations of 1D {@link BSPTree}s.
*/
public static class TreePrinter1D extends TreePrinter<Vector1D> {
@@ -271,8 +397,7 @@ public class EuclideanTestUtils {
write("cut = { hyperplane: ");
if (hyper.isDirect()) {
write("[" + hyper.getLocation().getX() + ", inf)");
- }
- else {
+ } else {
write("(-inf, " + hyper.getLocation().getX() + "]");
}
@@ -284,8 +409,7 @@ public class EuclideanTestUtils {
for (double[] interval : remainingRegion) {
if (isFirst) {
isFirst = false;
- }
- else {
+ } else {
write(", ");
}
write(Arrays.toString(interval));
@@ -298,7 +422,8 @@ public class EuclideanTestUtils {
}
}
- /** Class for creating string representations of 2D {@link BSPTree}s.
+ /**
+ * Class for creating string representations of 2D {@link BSPTree}s.
*/
public static class TreePrinter2D extends TreePrinter<Vector2D> {
@@ -316,8 +441,7 @@ public class EuclideanTestUtils {
for (double[] interval : remainingRegion) {
if (isFirst) {
isFirst = false;
- }
- else {
+ } else {
write(", ");
}
write(Arrays.toString(interval));
@@ -327,7 +451,8 @@ public class EuclideanTestUtils {
}
}
- /** Class for creating string representations of 3D {@link BSPTree}s.
+ /**
+ * Class for creating string representations of 3D {@link BSPTree}s.
*/
public static class TreePrinter3D extends TreePrinter<Vector3D> {
@@ -348,16 +473,14 @@ public class EuclideanTestUtils {
for (Vector2D vertex : loop) {
if (vertex != null) {
loop3.add(plane.toSpace(vertex));
- }
- else {
+ } else {
loop3.add(null);
}
}
if (isFirst) {
isFirst = false;
- }
- else {
+ } else {
write(", ");
}
diff --git a/commons-geometry-euclidean/src/test/java/org/apache/commons/geometry/euclidean/internal/MatricesTest.java b/commons-geometry-euclidean/src/test/java/org/apache/commons/geometry/euclidean/internal/MatricesTest.java
new file mode 100644
index 0000000..77fa3a4
--- /dev/null
+++ b/commons-geometry-euclidean/src/test/java/org/apache/commons/geometry/euclidean/internal/MatricesTest.java
@@ -0,0 +1,88 @@
+/*
+ * 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.geometry.euclidean.internal;
+
+
+import org.apache.commons.geometry.core.Geometry;
+import org.apache.commons.geometry.euclidean.threed.Vector3D;
+import org.apache.commons.geometry.euclidean.threed.rotation.QuaternionRotation;
+import org.junit.Assert;
+import org.junit.Test;
+
+
+public class MatricesTest {
+
+ private static final double EPS = 1e-12;
+
+ @Test
+ public void testDeterminant_2x2() {
+ // act/assert
+ Assert.assertEquals(1, Matrices.determinant(
+ 1, 0,
+ 0, 1), EPS);
+
+ Assert.assertEquals(-1, Matrices.determinant(
+ -1, 0,
+ 0, 1), EPS);
+
+ Assert.assertEquals(0, Matrices.determinant(
+ 1, 1,
+ 1, 1), EPS);
+
+ Assert.assertEquals(-2, Matrices.determinant(
+ 1, 2,
+ 3, 4), EPS);
+
+ Assert.assertEquals(7, Matrices.determinant(
+ -5, -4,
+ -2, -3), EPS);
+
+ Assert.assertEquals(9, Matrices.determinant(
+ -1, -2,
+ 6, 3), EPS);
+ }
+
+ @Test
+ public void testDeterminant_3x3() {
+ // act/assert
+ Assert.assertEquals(1, Matrices.determinant(
+ 1, 0, 0,
+ 0, 1, 0,
+ 0 , 0, 1), EPS);
+
+ Assert.assertEquals(-1, Matrices.determinant(
+ -1, 0, 0,
+ 0, -1, 0,
+ 0 , 0, -1), EPS);
+
+ Assert.assertEquals(0, Matrices.determinant(
+ 1, 2, 3,
+ 4, 5, 6,
+ 7, 8, 9), EPS);
+
+ Assert.assertEquals(49, Matrices.determinant(
+ 2, -3, 1,
+ 2, 0, -1,
+ 1, 4, 5), EPS);
+
+ Assert.assertEquals(-40, Matrices.determinant(
+ -5, 0, -1,
+ 1, 2, -1,
+ -3, 4, 1
+ ), EPS);
+ }
+}
diff --git a/commons-geometry-euclidean/src/test/java/org/apache/commons/geometry/euclidean/internal/VectorsTest.java b/commons-geometry-euclidean/src/test/java/org/apache/commons/geometry/euclidean/internal/VectorsTest.java
index e9ba2f3..d8f6bb4 100644
--- a/commons-geometry-euclidean/src/test/java/org/apache/commons/geometry/euclidean/internal/VectorsTest.java
+++ b/commons-geometry-euclidean/src/test/java/org/apache/commons/geometry/euclidean/internal/VectorsTest.java
@@ -124,6 +124,18 @@ public class VectorsTest {
}
@Test
+ public void testNorm_fourD() {
+ // act/assert
+ Assert.assertEquals(0.0, Vectors.norm(0.0, 0.0, 0.0, 0.0), EPS);
+
+ Assert.assertEquals(Math.sqrt(30.0), Vectors.norm(1.0, 2.0, 3.0, 4.0), EPS);
+ Assert.assertEquals(Math.sqrt(174.0), Vectors.norm(5.0, 6.0, 7.0, -8.0), EPS);
+ Assert.assertEquals(Math.sqrt(446.0), Vectors.norm(9.0, 10.0, -11.0, 12.0), EPS);
+ Assert.assertEquals(Math.sqrt(846.0), Vectors.norm(13.0, -14.0, 15.0, 16.0), EPS);
+ Assert.assertEquals(Math.sqrt(1374.0), Vectors.norm(-17.0, 18.0, 19.0, 20.0), EPS);
+ }
+
+ @Test
public void testNormSq_oneD() {
// act/assert
Assert.assertEquals(0.0, Vectors.normSq(0.0), EPS);
@@ -160,4 +172,16 @@ public class VectorsTest {
Assert.assertEquals(1202.0, Vectors.normSq(-19.0, -20.0, 21.0), EPS);
Assert.assertEquals(1589.0, Vectors.normSq(-22.0, -23.0, -24.0), EPS);
}
+
+ @Test
+ public void testNormSq_fourD() {
+ // act/assert
+ Assert.assertEquals(0.0, Vectors.normSq(0.0, 0.0, 0.0, 0.0), EPS);
+
+ Assert.assertEquals(30.0, Vectors.normSq(1.0, 2.0, 3.0, 4.0), EPS);
+ Assert.assertEquals(174.0, Vectors.normSq(5.0, 6.0, 7.0, -8.0), EPS);
+ Assert.assertEquals(446.0, Vectors.normSq(9.0, 10.0, -11.0, 12.0), EPS);
+ Assert.assertEquals(846.0, Vectors.normSq(13.0, -14.0, 15.0, 16.0), EPS);
+ Assert.assertEquals(1374.0, Vectors.normSq(-17.0, 18.0, 19.0, 20.0), EPS);
+ }
}
diff --git a/commons-geometry-euclidean/src/test/java/org/apache/commons/geometry/euclidean/oned/AffineTransformMatrix1DTest.java b/commons-geometry-euclidean/src/test/java/org/apache/commons/geometry/euclidean/oned/AffineTransformMatrix1DTest.java
new file mode 100644
index 0000000..fae0fb5
--- /dev/null
+++ b/commons-geometry-euclidean/src/test/java/org/apache/commons/geometry/euclidean/oned/AffineTransformMatrix1DTest.java
@@ -0,0 +1,649 @@
+/*
+ * 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.geometry.euclidean.oned;
+
+import org.apache.commons.geometry.core.Geometry;
+import org.apache.commons.geometry.core.GeometryTestUtils;
+import org.apache.commons.geometry.core.exception.IllegalNormException;
+import org.apache.commons.geometry.euclidean.EuclideanTestUtils;
+import org.apache.commons.geometry.euclidean.exception.NonInvertibleTransformException;
+import org.junit.Assert;
+import org.junit.Test;
+
+public class AffineTransformMatrix1DTest {
+
+ private static final double EPS = 1e-12;
+
+ @Test
+ public void testOf() {
+ // act
+ AffineTransformMatrix1D transform = AffineTransformMatrix1D.of(1, 2);
+
+ // assert
+ double[] result = transform.toArray();
+ Assert.assertArrayEquals(new double[] { 1, 2 }, result, 0.0);
+ }
+
+
+ @Test
+ public void testOf_invalidDimensions() {
+ // act/assert
+ GeometryTestUtils.assertThrows(() -> AffineTransformMatrix1D.of(1),
+ IllegalArgumentException.class, "Dimension mismatch: 1 != 2");
+ }
+
+ @Test
+ public void testIdentity() {
+ // act
+ AffineTransformMatrix1D transform = AffineTransformMatrix1D.identity();
+
+ // assert
+ double[] expected = { 1, 0 };
+ Assert.assertArrayEquals(expected, transform.toArray(), 0.0);
+ }
+
+ @Test
+ public void testCreateTranslation_value() {
+ // act
+ AffineTransformMatrix1D transform = AffineTransformMatrix1D.createTranslation(2);
+
+ // assert
+ double[] expected = { 1, 2 };
+ Assert.assertArrayEquals(expected, transform.toArray(), 0.0);
+ }
+
+ @Test
+ public void testCreateTranslation_vector() {
+ // act
+ AffineTransformMatrix1D transform = AffineTransformMatrix1D.createTranslation(Vector1D.of(5));
+
+ // assert
+ double[] expected = { 1, 5 };
+ Assert.assertArrayEquals(expected, transform.toArray(), 0.0);
+ }
+
+ @Test
+ public void testTranslate_value() {
+ // arrange
+ AffineTransformMatrix1D a = AffineTransformMatrix1D.of(2, 10);
+
+ // act
+ AffineTransformMatrix1D result = a.translate(4);
+
+ // assert
+ double[] expected = { 2, 14 };
+ Assert.assertArrayEquals(expected, result.toArray(), 0.0);
+ }
+
+ @Test
+ public void testTranslate_vector() {
+ // arrange
+ AffineTransformMatrix1D a = AffineTransformMatrix1D.of(2, 10);
+
+ // act
+ AffineTransformMatrix1D result = a.translate(Vector1D.of(7));
+
+ // assert
+ double[] expected = { 2, 17 };
+ Assert.assertArrayEquals(expected, result.toArray(), 0.0);
+ }
+
+ @Test
+ public void testCreateScale_vector() {
+ // act
+ AffineTransformMatrix1D transform = AffineTransformMatrix1D.createScale(Vector1D.of(4));
+
+ // assert
+ double[] expected = { 4, 0 };
+ Assert.assertArrayEquals(expected, transform.toArray(), 0.0);
+ }
+
+ @Test
+ public void testCreateScale_value() {
+ // act
+ AffineTransformMatrix1D transform = AffineTransformMatrix1D.createScale(7);
+
+ // assert
+ double[] expected = { 7, 0 };
+ Assert.assertArrayEquals(expected, transform.toArray(), 0.0);
+ }
+
+ @Test
+ public void testScale_value() {
+ // arrange
+ AffineTransformMatrix1D a = AffineTransformMatrix1D.of(2, 10);
+
+ // act
+ AffineTransformMatrix1D result = a.scale(4);
+
+ // assert
+ double[] expected = { 8, 40 };
+ Assert.assertArrayEquals(expected, result.toArray(), 0.0);
+ }
+
+ @Test
+ public void testScale_vector() {
+ // arrange
+ AffineTransformMatrix1D a = AffineTransformMatrix1D.of(2, 10);
+
+ // act
+ AffineTransformMatrix1D result = a.scale(Vector1D.of(7));
+
+ // assert
+ double[] expected = { 14, 70 };
+ Assert.assertArrayEquals(expected, result.toArray(), 0.0);
+ }
+
+ @Test
+ public void testApply_identity() {
+ // arrange
+ AffineTransformMatrix1D transform = AffineTransformMatrix1D.identity();
+
+ // act/assert
+ runWithCoordinates((x) -> {
+ Vector1D v = Vector1D.of(x);
+
+ EuclideanTestUtils.assertCoordinatesEqual(v, transform.apply(v), EPS);
+ });
+ }
+
+ @Test
+ public void testApply_translate() {
+ // arrange
+ Vector1D translation = Vector1D.of(-Geometry.PI);
+
+ AffineTransformMatrix1D transform = AffineTransformMatrix1D.identity()
+ .translate(translation);
+
+ // act/assert
+ runWithCoordinates((x) -> {
+ Vector1D vec = Vector1D.of(x);
+
+ Vector1D expectedVec = vec.add(translation);
+
+ EuclideanTestUtils.assertCoordinatesEqual(expectedVec, transform.apply(vec), EPS);
+ });
+ }
+
+ @Test
+ public void testApply_scale() {
+ // arrange
+ Vector1D factor = Vector1D.of(2.0);
+
+ AffineTransformMatrix1D transform = AffineTransformMatrix1D.identity()
+ .scale(factor);
+
+ // act/assert
+ runWithCoordinates((x) -> {
+ Vector1D vec = Vector1D.of(x);
+
+ Vector1D expectedVec = Vector1D.of(factor.getX() * x);
+
+ EuclideanTestUtils.assertCoordinatesEqual(expectedVec, transform.apply(vec), EPS);
+ });
+ }
+
+ @Test
+ public void testApply_translateThenScale() {
+ // arrange
+ Vector1D translation = Vector1D.of(-2.0);
+ Vector1D scale = Vector1D.of(5.0);
+
+ AffineTransformMatrix1D transform = AffineTransformMatrix1D.identity()
+ .translate(translation)
+ .scale(scale);
+
+ // act/assert
+ EuclideanTestUtils.assertCoordinatesEqual(Vector1D.of(-5), transform.apply(Vector1D.of(1)), EPS);
+
+ runWithCoordinates((x) -> {
+ Vector1D vec = Vector1D.of(x);
+
+ Vector1D expectedVec = Vector1D.of(
+ (x + translation.getX()) * scale.getX()
+ );
+
+ EuclideanTestUtils.assertCoordinatesEqual(expectedVec, transform.apply(vec), EPS);
+ });
+ }
+
+ @Test
+ public void testApply_scaleThenTranslate() {
+ // arrange
+ Vector1D scale = Vector1D.of(5.0);
+ Vector1D translation = Vector1D.of(-2.0);
+
+ AffineTransformMatrix1D transform = AffineTransformMatrix1D.identity()
+ .scale(scale)
+ .translate(translation);
+
+ // act/assert
+ runWithCoordinates((x) -> {
+ Vector1D vec = Vector1D.of(x);
+
+ Vector1D expectedVec = Vector1D.of(
+ (x * scale.getX()) + translation.getX()
+ );
+
+ EuclideanTestUtils.assertCoordinatesEqual(expectedVec, transform.apply(vec), EPS);
+ });
+ }
+
+ @Test
+ public void testApplyVector_identity() {
+ // arrange
+ AffineTransformMatrix1D transform = AffineTransformMatrix1D.identity();
+
+ // act/assert
+ runWithCoordinates((x) -> {
+ Vector1D v = Vector1D.of(x);
+
+ EuclideanTestUtils.assertCoordinatesEqual(v, transform.applyVector(v), EPS);
+ });
+ }
+
+ @Test
+ public void testApplyVector_translate() {
+ // arrange
+ Vector1D translation = Vector1D.of(-Geometry.PI);
+
+ AffineTransformMatrix1D transform = AffineTransformMatrix1D.identity()
+ .translate(translation);
+
+ // act/assert
+ runWithCoordinates((x) -> {
+ Vector1D vec = Vector1D.of(x);
+
+ EuclideanTestUtils.assertCoordinatesEqual(vec, transform.applyVector(vec), EPS);
+ });
+ }
+
+ @Test
+ public void testApplyVector_scale() {
+ // arrange
+ Vector1D factor = Vector1D.of(2.0);
+
+ AffineTransformMatrix1D transform = AffineTransformMatrix1D.identity()
+ .scale(factor);
+
+ // act/assert
+ runWithCoordinates((x) -> {
+ Vector1D vec = Vector1D.of(x);
+
+ Vector1D expectedVec = Vector1D.of(factor.getX() * x);
+
+ EuclideanTestUtils.assertCoordinatesEqual(expectedVec, transform.applyVector(vec), EPS);
+ });
+ }
+
+ @Test
+ public void testApplyVector_representsDisplacement() {
+ // arrange
+ Vector1D p1 = Vector1D.of(Geometry.PI);
+
+ Vector1D translation = Vector1D.of(-2.0);
+ Vector1D scale = Vector1D.of(5.0);
+
+ AffineTransformMatrix1D transform = AffineTransformMatrix1D.identity()
+ .translate(translation)
+ .scale(scale);
+
+ // act/assert
+ runWithCoordinates((x) -> {
+ Vector1D p2 = Vector1D.of(x);
+ Vector1D input = p1.subtract(p2);
+
+ Vector1D expectedVec = transform.apply(p1).subtract(transform.apply(p2));
+
+ EuclideanTestUtils.assertCoordinatesEqual(expectedVec, transform.applyVector(input), EPS);
+ });
+ }
+
+ @Test
+ public void testApplyDirection_identity() {
+ // arrange
+ AffineTransformMatrix1D transform = AffineTransformMatrix1D.identity();
+
+ // act/assert
+ runWithCoordinates((x) -> {
+ Vector1D v = Vector1D.of(x);
+
+ EuclideanTestUtils.assertCoordinatesEqual(v.normalize(), transform.applyDirection(v), EPS);
+ }, true);
+ }
+
+ @Test
+ public void testApplyDirection_translate() {
+ // arrange
+ Vector1D translation = Vector1D.of(-Geometry.PI);
+
+ AffineTransformMatrix1D transform = AffineTransformMatrix1D.identity()
+ .translate(translation);
+
+ // act/assert
+ runWithCoordinates((x) -> {
+ Vector1D vec = Vector1D.of(x);
+
+ EuclideanTestUtils.assertCoordinatesEqual(vec.normalize(), transform.applyDirection(vec), EPS);
+ }, true);
+ }
+
+ @Test
+ public void testApplyDirection_scale() {
+ // arrange
+ Vector1D factor = Vector1D.of(2.0);
+
+ AffineTransformMatrix1D transform = AffineTransformMatrix1D.identity()
+ .scale(factor);
+
+ // act/assert
+ runWithCoordinates((x) -> {
+ Vector1D vec = Vector1D.of(x);
+
+ Vector1D expectedVec = Vector1D.of(factor.getX() * x).normalize();
+
+ EuclideanTestUtils.assertCoordinatesEqual(expectedVec, transform.applyDirection(vec), EPS);
+ }, true);
+ }
+
+ @Test
+ public void testApplyDirection_representsNormalizedDisplacement() {
+ // arrange
+ Vector1D p1 = Vector1D.of(Geometry.PI);
+
+ Vector1D translation = Vector1D.of(-2.0);
+ Vector1D scale = Vector1D.of(5.0);
+
+ AffineTransformMatrix1D transform = AffineTransformMatrix1D.identity()
+ .translate(translation)
+ .scale(scale);
+
+ // act/assert
+ runWithCoordinates((x) -> {
+ Vector1D p2 = Vector1D.of(x);
+ Vector1D input = p1.subtract(p2);
+
+ Vector1D expectedVec = transform.apply(p1).subtract(transform.apply(p2)).normalize();
+
+ EuclideanTestUtils.assertCoordinatesEqual(expectedVec, transform.applyDirection(input), EPS);
+ });
+ }
+
+ @Test
+ public void testApplyDirection_illegalNorm() {
+ // act/assert
+ GeometryTestUtils.assertThrows(() -> AffineTransformMatrix1D.createScale(0).applyDirection(Vector1D.ONE),
+ IllegalNormException.class);
+ GeometryTestUtils.assertThrows(() -> AffineTransformMatrix1D.createScale(2).applyDirection(Vector1D.ZERO),
+ IllegalNormException.class);
+ }
+
+ @Test
+ public void testMultiply() {
+ // arrange
+ AffineTransformMatrix1D a = AffineTransformMatrix1D.of(2, 3);
+ AffineTransformMatrix1D b = AffineTransformMatrix1D.of(13, 14);
+
+ // act
+ AffineTransformMatrix1D result = a.multiply(b);
+
+ // assert
+ double[] arr = result.toArray();
+ Assert.assertArrayEquals(new double[] { 26, 31 }, arr, EPS);
+ }
+
+ @Test
+ public void testMultiply_combinesTransformOperations() {
+ // arrange
+ Vector1D translation1 = Vector1D.of(1);
+ double scale = 2.0;
+ Vector1D translation2 = Vector1D.of(4);
+
+ AffineTransformMatrix1D a = AffineTransformMatrix1D.createTranslation(translation1);
+ AffineTransformMatrix1D b = AffineTransformMatrix1D.createScale(scale);
+ AffineTransformMatrix1D c = AffineTransformMatrix1D.identity();
+ AffineTransformMatrix1D d = AffineTransformMatrix1D.createTranslation(translation2);
+
+ // act
+ AffineTransformMatrix1D transform = d.multiply(c).multiply(b).multiply(a);
+
+ // assert
+ runWithCoordinates((x) -> {
+ Vector1D vec = Vector1D.of(x);
+
+ Vector1D expectedVec = vec
+ .add(translation1)
+ .scalarMultiply(scale)
+ .add(translation2);
+
+ EuclideanTestUtils.assertCoordinatesEqual(expectedVec, transform.apply(vec), EPS);
+ });
+ }
+
+ @Test
+ public void testPremultiply() {
+ // arrange
+ AffineTransformMatrix1D a = AffineTransformMatrix1D.of(2, 3);
+ AffineTransformMatrix1D b = AffineTransformMatrix1D.of(13, 14);
+
+ // act
+ AffineTransformMatrix1D result = b.premultiply(a);
+
+ // assert
+ double[] arr = result.toArray();
+ Assert.assertArrayEquals(new double[] { 26, 31 }, arr, EPS);
+ }
+
+ @Test
+ public void testPremultiply_combinesTransformOperations() {
+ // arrange
+ Vector1D translation1 = Vector1D.of(1);
+ double scale = 2.0;
+ Vector1D translation2 = Vector1D.of(4);
+
+ AffineTransformMatrix1D a = AffineTransformMatrix1D.createTranslation(translation1);
+ AffineTransformMatrix1D b = AffineTransformMatrix1D.createScale(scale);
+ AffineTransformMatrix1D c = AffineTransformMatrix1D.identity();
+ AffineTransformMatrix1D d = AffineTransformMatrix1D.createTranslation(translation2);
+
+ // act
+ AffineTransformMatrix1D transform = a.premultiply(b).premultiply(c).premultiply(d);
+
+ // assert
+ runWithCoordinates((x) -> {
+ Vector1D vec = Vector1D.of(x);
+
+ Vector1D expectedVec = vec
+ .add(translation1)
+ .scalarMultiply(scale)
+ .add(translation2);
+
+ EuclideanTestUtils.assertCoordinatesEqual(expectedVec, transform.apply(vec), EPS);
+ });
+ }
+
+ @Test
+ public void testGetInverse_identity() {
+ // act
+ AffineTransformMatrix1D inverse = AffineTransformMatrix1D.identity().getInverse();
+
+ // assert
+ double[] expected = { 1, 0 };
+ Assert.assertArrayEquals(expected, inverse.toArray(), 0.0);
+ }
+
+ @Test
+ public void testGetInverse_multiplyByInverse_producesIdentity() {
+ // arrange
+ AffineTransformMatrix1D a = AffineTransformMatrix1D.of(1, 3);
+
+ AffineTransformMatrix1D inv = a.getInverse();
+
+ // act
+ AffineTransformMatrix1D result = inv.multiply(a);
+
+ // assert
+ double[] expected = { 1, 0 };
+ Assert.assertArrayEquals(expected, result.toArray(), EPS);
+ }
+
+ @Test
+ public void testGetInverse_translate() {
+ // arrange
+ AffineTransformMatrix1D transform = AffineTransformMatrix1D.createTranslation(3);
+
+ // act
+ AffineTransformMatrix1D inverse = transform.getInverse();
+
+ // assert
+ double[] expected = { 1, -3 };
+ Assert.assertArrayEquals(expected, inverse.toArray(), 0.0);
+ }
+
+ @Test
+ public void testGetInverse_scale() {
+ // arrange
+ AffineTransformMatrix1D transform = AffineTransformMatrix1D.createScale(10);
+
+ // act
+ AffineTransformMatrix1D inverse = transform.getInverse();
+
+ // assert
+ double[] expected = { 0.1, 0 };
+ Assert.assertArrayEquals(expected, inverse.toArray(), 0.0);
+ }
+
+ @Test
+ public void testGetInverse_undoesOriginalTransform_translationAndScale() {
+ // arrange
+ Vector1D v1 = Vector1D.ZERO;
+ Vector1D v2 = Vector1D.ONE;
+ Vector1D v3 = Vector1D.of(1.5);
+ Vector1D v4 = Vector1D.of(-2);
+
+ // act/assert
+ runWithCoordinates((x) -> {
+ AffineTransformMatrix1D transform = AffineTransformMatrix1D
+ .createTranslation(x)
+ .scale(2)
+ .translate(x / 3);
+
+ AffineTransformMatrix1D inverse = transform.getInverse();
+
+ EuclideanTestUtils.assertCoordinatesEqual(v1, inverse.apply(transform.apply(v1)), EPS);
+ EuclideanTestUtils.assertCoordinatesEqual(v2, inverse.apply(transform.apply(v2)), EPS);
+ EuclideanTestUtils.assertCoordinatesEqual(v3, inverse.apply(transform.apply(v3)), EPS);
+ EuclideanTestUtils.assertCoordinatesEqual(v4, inverse.apply(transform.apply(v4)), EPS);
+ });
+ }
+
+ @Test
+ public void testGetInverse_nonInvertible() {
+ // act/assert
+ GeometryTestUtils.assertThrows(() -> {
+ AffineTransformMatrix1D.of(0, 0).getInverse();
+ }, NonInvertibleTransformException.class, "Transform is not invertible; matrix determinant is 0.0");
+
+ GeometryTestUtils.assertThrows(() -> {
+ AffineTransformMatrix1D.of(Double.NaN, 0).getInverse();
+ }, NonInvertibleTransformException.class, "Transform is not invertible; matrix determinant is NaN");
+
+ GeometryTestUtils.assertThrows(() -> {
+ AffineTransformMatrix1D.of(Double.NEGATIVE_INFINITY, 0.0).getInverse();
+ }, NonInvertibleTransformException.class, "Transform is not invertible; matrix determinant is -Infinity");
+
+ GeometryTestUtils.assertThrows(() -> {
+ AffineTransformMatrix1D.of(Double.POSITIVE_INFINITY, 0).getInverse();
+ }, NonInvertibleTransformException.class, "Transform is not invertible; matrix determinant is Infinity");
+
+ GeometryTestUtils.assertThrows(() -> {
+ AffineTransformMatrix1D.of(1, Double.NaN).getInverse();
+ }, NonInvertibleTransformException.class, "Transform is not invertible; invalid matrix element: NaN");
+
+ GeometryTestUtils.assertThrows(() -> {
+ AffineTransformMatrix1D.of(1, Double.NEGATIVE_INFINITY).getInverse();
+ }, NonInvertibleTransformException.class, "Transform is not invertible; invalid matrix element: -Infinity");
+
+ GeometryTestUtils.assertThrows(() -> {
+ AffineTransformMatrix1D.of(1, Double.POSITIVE_INFINITY).getInverse();
+ }, NonInvertibleTransformException.class, "Transform is not invertible; invalid matrix element: Infinity");
+ }
+
+ @Test
+ public void testHashCode() {
+ // act
+ int orig = AffineTransformMatrix1D.of(1, 2).hashCode();
+ int same = AffineTransformMatrix1D.of(1, 2).hashCode();
+
+ // assert
+ Assert.assertEquals(orig, same);
+
+ Assert.assertNotEquals(orig, AffineTransformMatrix1D.of(0, 2).hashCode());
+ Assert.assertNotEquals(orig, AffineTransformMatrix1D.of(1, 0).hashCode());
+ }
+
+ @Test
+ public void testEquals() {
+ // arrange
+ AffineTransformMatrix1D a = AffineTransformMatrix1D.of(1, 2);
+
+ // act/assert
+ Assert.assertTrue(a.equals(a));
+
+ Assert.assertFalse(a.equals(null));
+ Assert.assertFalse(a.equals(new Object()));
+
+ Assert.assertFalse(a.equals(AffineTransformMatrix1D.of(0, 2)));
+ Assert.assertFalse(a.equals(AffineTransformMatrix1D.of(1, 0)));
+ }
+
+ @Test
+ public void testToString() {
+ // arrange
+ AffineTransformMatrix1D a = AffineTransformMatrix1D.of(1, 2);
+
+ // act
+ String result = a.toString();
+
+ // assert
+ Assert.assertEquals("[ 1.0, 2.0 ]", result);
+ }
+
+ @FunctionalInterface
+ private static interface Coordinate1DTest {
+
+ void run(double x);
+ }
+
+ private static void runWithCoordinates(Coordinate1DTest test) {
+ runWithCoordinates(test, false);
+ }
+
+ private static void runWithCoordinates(Coordinate1DTest test, boolean skipZero) {
+ runWithCoordinates(test, -1e-2, 1e-2, 5e-3, skipZero);
+ runWithCoordinates(test, -1e2, 1e2, 5, skipZero);
+ }
+
+ private static void runWithCoordinates(Coordinate1DTest test, double min, double max, double step, boolean skipZero)
+ {
+ for (double x = min; x <= max; x += step) {
+ if (!skipZero || x != 0.0) {
+ test.run(x);
+ }
+ }
+ }
+}
diff --git a/commons-geometry-euclidean/src/test/java/org/apache/commons/geometry/euclidean/oned/IntervalTest.java b/commons-geometry-euclidean/src/test/java/org/apache/commons/geometry/euclidean/oned/IntervalTest.java
index 55a9eb0..ac808a6 100644
--- a/commons-geometry-euclidean/src/test/java/org/apache/commons/geometry/euclidean/oned/IntervalTest.java
+++ b/commons-geometry-euclidean/src/test/java/org/apache/commons/geometry/euclidean/oned/IntervalTest.java
@@ -18,7 +18,6 @@ package org.apache.commons.geometry.euclidean.oned;
import org.apache.commons.geometry.core.partitioning.Region;
import org.apache.commons.geometry.euclidean.EuclideanTestUtils;
-import org.apache.commons.geometry.euclidean.oned.Interval;
import org.apache.commons.numbers.core.Precision;
import org.junit.Assert;
import org.junit.Test;
diff --git a/commons-geometry-euclidean/src/test/java/org/apache/commons/geometry/euclidean/oned/Vector1DTest.java b/commons-geometry-euclidean/src/test/java/org/apache/commons/geometry/euclidean/oned/Vector1DTest.java
index 60241e6..7a74df8 100644
--- a/commons-geometry-euclidean/src/test/java/org/apache/commons/geometry/euclidean/oned/Vector1DTest.java
+++ b/commons-geometry-euclidean/src/test/java/org/apache/commons/geometry/euclidean/oned/Vector1DTest.java
@@ -449,6 +449,21 @@ public class Vector1DTest {
}
@Test
+ public void testTransform() {
+ // arrange
+ AffineTransformMatrix1D transform = AffineTransformMatrix1D.identity()
+ .scale(2)
+ .translate(1);
+
+ Vector1D v1 = Vector1D.of(1);
+ Vector1D v2 = Vector1D.of(-4);
+
+ // act/assert
+ checkVector(v1.transform(transform), 3);
+ checkVector(v2.transform(transform), -7);
+ }
+
+ @Test
public void testHashCode() {
// arrange
Vector1D u = Vector1D.of(1);
diff --git a/commons-geometry-euclidean/src/test/java/org/apache/commons/geometry/euclidean/threed/AffineTransformMatrix3DTest.java b/commons-geometry-euclidean/src/test/java/org/apache/commons/geometry/euclidean/threed/AffineTransformMatrix3DTest.java
new file mode 100644
index 0000000..115b114
--- /dev/null
+++ b/commons-geometry-euclidean/src/test/java/org/apache/commons/geometry/euclidean/threed/AffineTransformMatrix3DTest.java
@@ -0,0 +1,957 @@
+/*
+ * 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.geometry.euclidean.threed;
+
+import org.apache.commons.geometry.core.Geometry;
+import org.apache.commons.geometry.core.GeometryTestUtils;
+import org.apache.commons.geometry.core.exception.IllegalNormException;
+import org.apache.commons.geometry.euclidean.EuclideanTestUtils;
+import org.apache.commons.geometry.euclidean.EuclideanTestUtils.PermuteCallback3D;
+import org.apache.commons.geometry.euclidean.exception.NonInvertibleTransformException;
+import org.apache.commons.geometry.euclidean.threed.rotation.QuaternionRotation;
+import org.apache.commons.geometry.euclidean.threed.rotation.StandardRotations;
+import org.junit.Assert;
+import org.junit.Test;
+
+public class AffineTransformMatrix3DTest {
+
+ private static final double EPS = 1e-12;
+
+ @Test
+ public void testOf() {
+ // arrange
+ double[] arr = {
+ 1, 2, 3, 4,
+ 5, 6, 7, 8,
+ 9, 10, 11, 12
+ };
+
+ // act
+ AffineTransformMatrix3D transform = AffineTransformMatrix3D.of(arr);
+
+ // assert
+ double[] result = transform.toArray();
+ Assert.assertNotSame(arr, result);
+ Assert.assertArrayEquals(arr, result, 0.0);
+ }
+
+ @Test
+ public void testOf_invalidDimensions() {
+ // act/assert
+ GeometryTestUtils.assertThrows(() -> AffineTransformMatrix3D.of(1, 2),
+ IllegalArgumentException.class, "Dimension mismatch: 2 != 12");
+ }
+
+ @Test
+ public void testIdentity() {
+ // act
+ AffineTransformMatrix3D transform = AffineTransformMatrix3D.identity();
+
+ // assert
+ double[] expected = {
+ 1, 0, 0, 0,
+ 0, 1, 0, 0,
+ 0, 0, 1, 0
+ };
+ Assert.assertArrayEquals(expected, transform.toArray(), 0.0);
+ }
+
+ @Test
+ public void testCreateTranslation_xyz() {
+ // act
+ AffineTransformMatrix3D transform = AffineTransformMatrix3D.createTranslation(2, 3, 4);
+
+ // assert
+ double[] expected = {
+ 1, 0, 0, 2,
+ 0, 1, 0, 3,
+ 0, 0, 1, 4
+ };
+ Assert.assertArrayEquals(expected, transform.toArray(), 0.0);
+ }
+
+ @Test
+ public void testCreateTranslation_vector() {
+ // act
+ AffineTransformMatrix3D transform = AffineTransformMatrix3D.createTranslation(Vector3D.of(5, 6, 7));
+
+ // assert
+ double[] expected = {
+ 1, 0, 0, 5,
+ 0, 1, 0, 6,
+ 0, 0, 1, 7
+ };
+ Assert.assertArrayEquals(expected, transform.toArray(), 0.0);
+ }
+
+ @Test
+ public void testCreateScale_xyz() {
+ // act
+ AffineTransformMatrix3D transform = AffineTransformMatrix3D.createScale(2, 3, 4);
+
+ // assert
+ double[] expected = {
+ 2, 0, 0, 0,
+ 0, 3, 0, 0,
+ 0, 0, 4, 0
+ };
+ Assert.assertArrayEquals(expected, transform.toArray(), 0.0);
+ }
+
+ @Test
+ public void testTranslate_xyz() {
+ // arrange
+ AffineTransformMatrix3D a = AffineTransformMatrix3D.of(
+ 2, 0, 0, 10,
+ 0, 3, 0, 11,
+ 0, 0, 4, 12
+ );
+
+ // act
+ AffineTransformMatrix3D result = a.translate(4, 5, 6);
+
+ // assert
+ double[] expected = {
+ 2, 0, 0, 14,
+ 0, 3, 0, 16,
+ 0, 0, 4, 18
+ };
+ Assert.assertArrayEquals(expected, result.toArray(), 0.0);
+ }
+
+ @Test
+ public void testTranslate_vector() {
+ // arrange
+ AffineTransformMatrix3D a = AffineTransformMatrix3D.of(
+ 2, 0, 0, 10,
+ 0, 3, 0, 11,
+ 0, 0, 4, 12
+ );
+
+ // act
+ AffineTransformMatrix3D result = a.translate(Vector3D.of(7, 8, 9));
+
+ // assert
+ double[] expected = {
+ 2, 0, 0, 17,
+ 0, 3, 0, 19,
+ 0, 0, 4, 21
+ };
+ Assert.assertArrayEquals(expected, result.toArray(), 0.0);
+ }
+
+ @Test
+ public void testCreateScale_vector() {
+ // act
+ AffineTransformMatrix3D transform = AffineTransformMatrix3D.createScale(Vector3D.of(4, 5, 6));
+
+ // assert
+ double[] expected = {
+ 4, 0, 0, 0,
+ 0, 5, 0, 0,
+ 0, 0, 6, 0
+ };
+ Assert.assertArrayEquals(expected, transform.toArray(), 0.0);
+ }
+
+ @Test
+ public void testCreateScale_singleValue() {
+ // act
+ AffineTransformMatrix3D transform = AffineTransformMatrix3D.createScale(7);
+
+ // assert
+ double[] expected = {
+ 7, 0, 0, 0,
+ 0, 7, 0, 0,
+ 0, 0, 7, 0
+ };
+ Assert.assertArrayEquals(expected, transform.toArray(), 0.0);
+ }
+
+ @Test
+ public void testScale_xyz() {
+ // arrange
+ AffineTransformMatrix3D a = AffineTransformMatrix3D.of(
+ 2, 0, 0, 10,
+ 0, 3, 0, 11,
+ 0, 0, 4, 12
+ );
+
+ // act
+ AffineTransformMatrix3D result = a.scale(4, 5, 6);
+
+ // assert
+ double[] expected = {
+ 8, 0, 0, 40,
+ 0, 15, 0, 55,
+ 0, 0, 24, 72
+ };
+ Assert.assertArrayEquals(expected, result.toArray(), 0.0);
+ }
+
+ @Test
+ public void testScale_vector() {
+ // arrange
+ AffineTransformMatrix3D a = AffineTransformMatrix3D.of(
+ 2, 0, 0, 10,
+ 0, 3, 0, 11,
+ 0, 0, 4, 12
+ );
+
+ // act
+ AffineTransformMatrix3D result = a.scale(Vector3D.of(7, 8, 9));
+
+ // assert
+ double[] expected = {
+ 14, 0, 0, 70,
+ 0, 24, 0, 88,
+ 0, 0, 36, 108
+ };
+ Assert.assertArrayEquals(expected, result.toArray(), 0.0);
+ }
+
+ @Test
+ public void testScale_singleValue() {
+ // arrange
+ AffineTransformMatrix3D a = AffineTransformMatrix3D.of(
+ 2, 0, 0, 10,
+ 0, 3, 0, 11,
+ 0, 0, 4, 12
+ );
+
+ // act
+ AffineTransformMatrix3D result = a.scale(10);
+
+ // assert
+ double[] expected = {
+ 20, 0, 0, 100,
+ 0, 30, 0, 110,
+ 0, 0, 40, 120
+ };
+ Assert.assertArrayEquals(expected, result.toArray(), 0.0);
+ }
+
+ @Test
+ public void testCreateRotation() {
+ // arrange
+ Vector3D center = Vector3D.of(1, 2, 3);
+ QuaternionRotation rotation = QuaternionRotation.fromAxisAngle(Vector3D.PLUS_Z, Geometry.HALF_PI);
+
+ // act
+ AffineTransformMatrix3D result = AffineTransformMatrix3D.createRotation(center, rotation);
+
+ // assert
+ double[] expected = {
+ 0, -1, 0, 3,
+ 1, 0, 0, 1,
+ 0, 0, 1, 0
+ };
+ Assert.assertArrayEquals(expected, result.toArray(), EPS);
+ }
+
+ @Test
+ public void testRotate() {
+ // arrange
+ AffineTransformMatrix3D a = AffineTransformMatrix3D.of(
+ 1, 2, 3, 4,
+ 5, 6, 7, 8,
+ 9, 10, 11, 12
+ );
+
+ QuaternionRotation rotation = QuaternionRotation.fromAxisAngle(Vector3D.PLUS_Z, Geometry.HALF_PI);
+
+ // act
+ AffineTransformMatrix3D result = a.rotate(rotation);
+
+ // assert
+ double[] expected = {
+ -5, -6, -7, -8,
+ 1, 2, 3, 4,
+ 9, 10, 11, 12
+ };
+ Assert.assertArrayEquals(expected, result.toArray(), EPS);
+ }
+
+ @Test
+ public void testRotate_aroundCenter() {
+ // arrange
+ AffineTransformMatrix3D a = AffineTransformMatrix3D.of(
+ 1, 2, 3, 4,
+ 5, 6, 7, 8,
+ 9, 10, 11, 12
+ );
+
+ Vector3D center = Vector3D.of(1, 2, 3);
+ QuaternionRotation rotation = QuaternionRotation.fromAxisAngle(Vector3D.PLUS_Z, Geometry.HALF_PI);
+
+ // act
+ AffineTransformMatrix3D result = a.rotate(center, rotation);
+
+ // assert
+ double[] expected = {
+ -5, -6, -7, -5,
+ 1, 2, 3, 5,
+ 9, 10, 11, 12
+ };
+ Assert.assertArrayEquals(expected, result.toArray(), EPS);
+ }
+
+ @Test
+ public void testApply_identity() {
+ // arrange
+ AffineTransformMatrix3D transform = AffineTransformMatrix3D.identity();
+
+ // act/assert
+ runWithCoordinates((x, y, z) -> {
+ Vector3D v = Vector3D.of(x, y, z);
+
+ EuclideanTestUtils.assertCoordinatesEqual(v, transform.apply(v), EPS);
+ });
+ }
+
+ @Test
+ public void testApply_translate() {
+ // arrange
+ Vector3D translation = Vector3D.of(1.1, -Geometry.PI, 5.5);
+
+ AffineTransformMatrix3D transform = AffineTransformMatrix3D.identity()
+ .translate(translation);
+
+ // act/assert
+ runWithCoordinates((x, y, z) -> {
+ Vector3D vec = Vector3D.of(x, y, z);
+
+ Vector3D expectedVec = vec.add(translation);
+
+ EuclideanTestUtils.assertCoordinatesEqual(expectedVec, transform.apply(vec), EPS);
+ });
+ }
+
+ @Test
+ public void testApply_scale() {
+ // arrange
+ Vector3D factors = Vector3D.of(2.0, -3.0, 4.0);
+
+ AffineTransformMatrix3D transform = AffineTransformMatrix3D.identity()
+ .scale(factors);
+
+ // act/assert
+ runWithCoordinates((x, y, z) -> {
+ Vector3D vec = Vector3D.of(x, y, z);
+
+ Vector3D expectedVec = Vector3D.of(factors.getX() * x, factors.getY() * y, factors.getZ() * z);
+
+ EuclideanTestUtils.assertCoordinatesEqual(expectedVec, transform.apply(vec), EPS);
+ });
+ }
+
+ @Test
+ public void testApply_translateThenScale() {
+ // arrange
+ Vector3D translation = Vector3D.of(-2.0, -3.0, -4.0);
+ Vector3D scale = Vector3D.of(5.0, 6.0, 7.0);
+
+ AffineTransformMatrix3D transform = AffineTransformMatrix3D.identity()
+ .translate(translation)
+ .scale(scale);
+
+ // act/assert
+ EuclideanTestUtils.assertCoordinatesEqual(Vector3D.of(-5, -12, -21), transform.apply(Vector3D.of(1, 1, 1)), EPS);
+
+ runWithCoordinates((x, y, z) -> {
+ Vector3D vec = Vector3D.of(x, y, z);
+
+ Vector3D expectedVec = Vector3D.of(
+ (x + translation.getX()) * scale.getX(),
+ (y + translation.getY()) * scale.getY(),
+ (z + translation.getZ()) * scale.getZ()
+ );
+
+ EuclideanTestUtils.assertCoordinatesEqual(expectedVec, transform.apply(vec), EPS);
+ });
+ }
+
+ @Test
+ public void testApply_scaleThenTranslate() {
+ // arrange
+ Vector3D scale = Vector3D.of(5.0, 6.0, 7.0);
+ Vector3D translation = Vector3D.of(-2.0, -3.0, -4.0);
+
+ AffineTransformMatrix3D transform = AffineTransformMatrix3D.identity()
+ .scale(scale)
+ .translate(translation);
+
+ // act/assert
+ runWithCoordinates((x, y, z) -> {
+ Vector3D vec = Vector3D.of(x, y, z);
+
+ Vector3D expectedVec = Vector3D.of(
+ (x * scale.getX()) + translation.getX(),
+ (y * scale.getY()) + translation.getY(),
+ (z * scale.getZ()) + translation.getZ()
+ );
+
+ EuclideanTestUtils.assertCoordinatesEqual(expectedVec, transform.apply(vec), EPS);
+ });
+ }
+
+ @Test
+ public void testApply_rotate() {
+ // arrange
+ QuaternionRotation rotation = QuaternionRotation.fromAxisAngle(Vector3D.of(1, 1, 1), 2.0 * Geometry.PI / 3.0);
+
+ AffineTransformMatrix3D transform = AffineTransformMatrix3D.identity().rotate(rotation);
+
+ // act/assert
+ runWithCoordinates((x, y, z) -> {
+ Vector3D vec = Vector3D.of(x, y, z);
+
+ Vector3D expectedVec = StandardRotations.PLUS_DIAGONAL_TWO_THIRDS_PI.apply(vec);
+
+ EuclideanTestUtils.assertCoordinatesEqual(expectedVec, transform.apply(vec), EPS);
+ });
+ }
+
+ @Test
+ public void testApply_rotate_aroundCenter() {
+ // arrange
+ double scaleFactor = 2;
+ Vector3D center = Vector3D.of(3, -4, 5);
+ QuaternionRotation rotation = QuaternionRotation.fromAxisAngle(Vector3D.PLUS_Z, Geometry.HALF_PI);
+
+ AffineTransformMatrix3D transform = AffineTransformMatrix3D.identity()
+ .scale(scaleFactor)
+ .rotate(center, rotation);
+
+ // act/assert
+ EuclideanTestUtils.assertCoordinatesEqual(Vector3D.of(3, -3, 2), transform.apply(Vector3D.of(2, -2, 1)), EPS);
+
+ runWithCoordinates((x, y, z) -> {
+ Vector3D vec = Vector3D.of(x, y, z);
+
+ Vector3D expectedVec = StandardRotations.PLUS_Z_HALF_PI.apply(vec.scalarMultiply(scaleFactor).subtract(center)).add(center);
+
+ EuclideanTestUtils.assertCoordinatesEqual(expectedVec, transform.apply(vec), EPS);
+ });
+ }
+
+ @Test
+ public void testApplyVector_identity() {
+ // arrange
+ AffineTransformMatrix3D transform = AffineTransformMatrix3D.identity();
+
+ // act/assert
+ runWithCoordinates((x, y, z) -> {
+ Vector3D v = Vector3D.of(x, y, z);
+
+ EuclideanTestUtils.assertCoordinatesEqual(v, transform.applyVector(v), EPS);
+ });
+ }
+
+ @Test
+ public void testApplyVector_translate() {
+ // arrange
+ Vector3D translation = Vector3D.of(1.1, -Geometry.PI, 5.5);
+
+ AffineTransformMatrix3D transform = AffineTransformMatrix3D.identity()
+ .translate(translation);
+
+ // act/assert
+ runWithCoordinates((x, y, z) -> {
+ Vector3D vec = Vector3D.of(x, y, z);
+
+ EuclideanTestUtils.assertCoordinatesEqual(vec, transform.applyVector(vec), EPS);
+ });
+ }
+
+ @Test
+ public void testApplyVector_scale() {
+ // arrange
+ Vector3D factors = Vector3D.of(2.0, -3.0, 4.0);
+
+ AffineTransformMatrix3D transform = AffineTransformMatrix3D.identity()
+ .scale(factors);
+
+ // act/assert
+ runWithCoordinates((x, y, z) -> {
+ Vector3D vec = Vector3D.of(x, y, z);
+
+ Vector3D expectedVec = Vector3D.of(factors.getX() * x, factors.getY() * y, factors.getZ() * z);
+
+ EuclideanTestUtils.assertCoordinatesEqual(expectedVec, transform.applyVector(vec), EPS);
+ });
+ }
+
+ @Test
+ public void testApplyVector_representsDisplacement() {
+ // arrange
+ Vector3D p1 = Vector3D.of(1, 2, 3);
+
+ AffineTransformMatrix3D transform = AffineTransformMatrix3D.identity()
+ .scale(1.5)
+ .translate(4, 6, 5)
+ .rotate(QuaternionRotation.fromAxisAngle(Vector3D.PLUS_Z, Geometry.HALF_PI));
+
+ // act/assert
+ runWithCoordinates((x, y, z) -> {
+ Vector3D p2 = Vector3D.of(x, y, z);
+ Vector3D input = p1.subtract(p2);
+
+ Vector3D expected = transform.apply(p1).subtract(transform.apply(p2));
+
+ EuclideanTestUtils.assertCoordinatesEqual(expected, transform.applyVector(input), EPS);
+ });
+ }
+
+ @Test
+ public void testApplyDirection_identity() {
+ // arrange
+ AffineTransformMatrix3D transform = AffineTransformMatrix3D.identity();
+
+ // act/assert
+ EuclideanTestUtils.permuteSkipZero(-5, 5, 0.5, (x, y, z) -> {
+ Vector3D v = Vector3D.of(x, y, z);
+
+ EuclideanTestUtils.assertCoordinatesEqual(v.normalize(), transform.applyDirection(v), EPS);
+ });
+ }
+
+ @Test
+ public void testApplyDirection_translate() {
+ // arrange
+ Vector3D translation = Vector3D.of(1.1, -Geometry.PI, 5.5);
+
+ AffineTransformMatrix3D transform = AffineTransformMatrix3D.identity()
+ .translate(translation);
+
+ // act/assert
+ EuclideanTestUtils.permuteSkipZero(-5, 5, 0.5, (x, y, z) -> {
+ Vector3D vec = Vector3D.of(x, y, z);
+
+ EuclideanTestUtils.assertCoordinatesEqual(vec.normalize(), transform.applyDirection(vec), EPS);
+ });
+ }
+
+ @Test
+ public void testApplyDirection_scale() {
+ // arrange
+ Vector3D factors = Vector3D.of(2.0, -3.0, 4.0);
+
+ AffineTransformMatrix3D transform = AffineTransformMatrix3D.identity()
+ .scale(factors);
+
+ // act/assert
+ EuclideanTestUtils.permuteSkipZero(-5, 5, 0.5, (x, y, z) -> {
+ Vector3D vec = Vector3D.of(x, y, z);
+
+ Vector3D expectedVec = Vector3D.of(factors.getX() * x, factors.getY() * y, factors.getZ() * z).normalize();
+
+ EuclideanTestUtils.assertCoordinatesEqual(expectedVec, transform.applyDirection(vec), EPS);
+ });
+ }
+
+ @Test
+ public void testApplyDirection_representsNormalizedDisplacement() {
+ // arrange
+ Vector3D p1 = Vector3D.of(1, 2, 3);
+
+ AffineTransformMatrix3D transform = AffineTransformMatrix3D.identity()
+ .scale(1.5)
+ .translate(4, 6, 5)
+ .rotate(QuaternionRotation.fromAxisAngle(Vector3D.PLUS_Z, Geometry.HALF_PI));
+
+ // act/assert
+ runWithCoordinates((x, y, z) -> {
+ Vector3D p2 = Vector3D.of(x, y, z);
+ Vector3D input = p1.subtract(p2);
+
+ Vector3D expected = transform.apply(p1).subtract(transform.apply(p2)).normalize();
+
+ EuclideanTestUtils.assertCoordinatesEqual(expected, transform.applyDirection(input), EPS);
+ });
+ }
+
+ @Test
+ public void testApplyDirection_illegalNorm() {
+ // act/assert
+ GeometryTestUtils.assertThrows(() -> AffineTransformMatrix3D.createScale(1, 0, 1).applyDirection(Vector3D.PLUS_Y),
+ IllegalNormException.class);
+ GeometryTestUtils.assertThrows(() -> AffineTransformMatrix3D.createScale(2).applyDirection(Vector3D.ZERO),
+ IllegalNormException.class);
+ }
+
+ @Test
+ public void testMultiply() {
+ // arrange
+ AffineTransformMatrix3D a = AffineTransformMatrix3D.of(
+ 1, 2, 3, 4,
+ 5, 6, 7, 8,
+ 9, 10, 11, 12
+ );
+ AffineTransformMatrix3D b = AffineTransformMatrix3D.of(
+ 13, 14, 15, 16,
+ 17, 18, 19, 20,
+ 21, 22, 23, 24
+ );
+
+ // act
+ AffineTransformMatrix3D result = a.multiply(b);
+
+ // assert
+ double[] arr = result.toArray();
+ Assert.assertArrayEquals(new double[] {
+ 110, 116, 122, 132,
+ 314, 332, 350, 376,
+ 518, 548, 578, 620
+ }, arr, EPS);
+ }
+
+ @Test
+ public void testMultiply_combinesTransformOperations() {
+ // arrange
+ Vector3D translation1 = Vector3D.of(1, 2, 3);
+ double scale = 2.0;
+ Vector3D translation2 = Vector3D.of(4, 5, 6);
+
+ AffineTransformMatrix3D a = AffineTransformMatrix3D.createTranslation(translation1);
+ AffineTransformMatrix3D b = AffineTransformMatrix3D.createScale(scale);
+ AffineTransformMatrix3D c = AffineTransformMatrix3D.identity();
+ AffineTransformMatrix3D d = AffineTransformMatrix3D.createTranslation(translation2);
+
+ // act
+ AffineTransformMatrix3D transform = d.multiply(c).multiply(b).multiply(a);
+
+ // assert
+ runWithCoordinates((x, y, z) -> {
+ Vector3D vec = Vector3D.of(x, y, z);
+
+ Vector3D expectedVec = vec
+ .add(translation1)
+ .scalarMultiply(scale)
+ .add(translation2);
+
+ EuclideanTestUtils.assertCoordinatesEqual(expectedVec, transform.apply(vec), EPS);
+ });
+ }
+
+ @Test
+ public void testPremultiply() {
+ // arrange
+ AffineTransformMatrix3D a = AffineTransformMatrix3D.of(
+ 1, 2, 3, 4,
+ 5, 6, 7, 8,
+ 9, 10, 11, 12
+ );
+ AffineTransformMatrix3D b = AffineTransformMatrix3D.of(
+ 13, 14, 15, 16,
+ 17, 18, 19, 20,
+ 21, 22, 23, 24
+ );
+
+ // act
+ AffineTransformMatrix3D result = b.premultiply(a);
+
+ // assert
+ double[] arr = result.toArray();
+ Assert.assertArrayEquals(new double[] {
+ 110, 116, 122, 132,
+ 314, 332, 350, 376,
+ 518, 548, 578, 620
+ }, arr, EPS);
+ }
+
+ @Test
+ public void testPremultiply_combinesTransformOperations() {
+ // arrange
+ Vector3D translation1 = Vector3D.of(1, 2, 3);
+ double scale = 2.0;
+ Vector3D translation2 = Vector3D.of(4, 5, 6);
+
+ AffineTransformMatrix3D a = AffineTransformMatrix3D.createTranslation(translation1);
+ AffineTransformMatrix3D b = AffineTransformMatrix3D.createScale(scale);
+ AffineTransformMatrix3D c = AffineTransformMatrix3D.identity();
+ AffineTransformMatrix3D d = AffineTransformMatrix3D.createTranslation(translation2);
+
+ // act
+ AffineTransformMatrix3D transform = a.premultiply(b).premultiply(c).premultiply(d);
+
+ // assert
+ runWithCoordinates((x, y, z) -> {
+ Vector3D vec = Vector3D.of(x, y, z);
+
+ Vector3D expectedVec = vec
+ .add(translation1)
+ .scalarMultiply(scale)
+ .add(translation2);
+
+ EuclideanTestUtils.assertCoordinatesEqual(expectedVec, transform.apply(vec), EPS);
+ });
+ }
+
+ @Test
+ public void testGetInverse_identity() {
+ // act
+ AffineTransformMatrix3D inverse = AffineTransformMatrix3D.identity().getInverse();
+
+ // assert
+ double[] expected = {
+ 1, 0, 0, 0,
+ 0, 1, 0, 0,
+ 0, 0, 1, 0
+ };
+ Assert.assertArrayEquals(expected, inverse.toArray(), 0.0);
+ }
+
+ @Test
+ public void testGetInverse_multiplyByInverse_producesIdentity() {
+ // arrange
+ AffineTransformMatrix3D a = AffineTransformMatrix3D.of(
+ 1, 3, 7, 8,
+ 2, 4, 9, 12,
+ 5, 6, 10, 11
+ );
+
+ AffineTransformMatrix3D inv = a.getInverse();
+
+ // act
+ AffineTransformMatrix3D result = inv.multiply(a);
+
+ // assert
+ double[] expected = {
+ 1, 0, 0, 0,
+ 0, 1, 0, 0,
+ 0, 0, 1, 0
+ };
+ Assert.assertArrayEquals(expected, result.toArray(), EPS);
+ }
+
+ @Test
+ public void testGetInverse_translate() {
+ // arrange
+ AffineTransformMatrix3D transform = AffineTransformMatrix3D.createTranslation(1, -2, 4);
+
+ // act
+ AffineTransformMatrix3D inverse = transform.getInverse();
+
+ // assert
+ double[] expected = {
+ 1, 0, 0, -1,
+ 0, 1, 0, 2,
+ 0, 0, 1, -4
+ };
+ Assert.assertArrayEquals(expected, inverse.toArray(), 0.0);
+ }
+
+ @Test
+ public void testGetInverse_scale() {
+ // arrange
+ AffineTransformMatrix3D transform = AffineTransformMatrix3D.createScale(10, -2, 4);
+
+ // act
+ AffineTransformMatrix3D inverse = transform.getInverse();
+
+ // assert
+ double[] expected = {
+ 0.1, 0, 0, 0,
+ 0, -0.5, 0, 0,
+ 0, 0, 0.25, 0
+ };
+ Assert.assertArrayEquals(expected, inverse.toArray(), 0.0);
+ }
+
+ @Test
+ public void testGetInverse_rotate() {
+ // arrange
+ Vector3D center = Vector3D.of(1, 2, 3);
+ QuaternionRotation rotation = QuaternionRotation.fromAxisAngle(Vector3D.PLUS_Z, Geometry.HALF_PI);
+
+ AffineTransformMatrix3D transform = AffineTransformMatrix3D.createRotation(center, rotation);
+
+ // act
+ AffineTransformMatrix3D inverse = transform.getInverse();
+
+ // assert
+ double[] expected = {
+ 0, 1, 0, -1,
+ -1, 0, 0, 3,
+ 0, 0, 1, 0
+ };
+ Assert.assertArrayEquals(expected, inverse.toArray(), EPS);
+ }
+
+ @Test
+ public void testGetInverse_undoesOriginalTransform() {
+ // arrange
+ Vector3D v1 = Vector3D.ZERO;
+ Vector3D v2 = Vector3D.PLUS_X;
+ Vector3D v3 = Vector3D.of(1, 1, 1);
+ Vector3D v4 = Vector3D.of(-2, 3, 4);
+
+ Vector3D center = Vector3D.of(1, 2, 3);
+ QuaternionRotation rotation = QuaternionRotation.fromAxisAngle(Vector3D.of(1, 2, 3), 0.25);
+
+ // act/assert
+ runWithCoordinates((x, y, z) -> {
+ AffineTransformMatrix3D transform = AffineTransformMatrix3D
+ .createTranslation(x, y, z)
+ .scale(2, 3, 4)
+ .rotate(center, rotation)
+ .translate(x / 3, y / 3, z / 3);
+
+ AffineTransformMatrix3D inverse = transform.getInverse();
+
+ EuclideanTestUtils.assertCoordinatesEqual(v1, inverse.apply(transform.apply(v1)), EPS);
+ EuclideanTestUtils.assertCoordinatesEqual(v2, inverse.apply(transform.apply(v2)), EPS);
+ EuclideanTestUtils.assertCoordinatesEqual(v3, inverse.apply(transform.apply(v3)), EPS);
+ EuclideanTestUtils.assertCoordinatesEqual(v4, inverse.apply(transform.apply(v4)), EPS);
+ });
+ }
+
+ @Test
+ public void testGetInverse_nonInvertible() {
+ // act/assert
+ GeometryTestUtils.assertThrows(() -> {
+ AffineTransformMatrix3D.of(
+ 0, 0, 0, 0,
+ 0, 0, 0, 0,
+ 0, 0, 0, 0).getInverse();
+ }, NonInvertibleTransformException.class, "Transform is not invertible; matrix determinant is 0.0");
+
+ GeometryTestUtils.assertThrows(() -> {
+ AffineTransformMatrix3D.of(
+ 1, 0, 0, 0,
+ 0, 1, 0, 0,
+ 0, 0, Double.NaN, 0).getInverse();
+ }, NonInvertibleTransformException.class, "Transform is not invertible; matrix determinant is NaN");
+
+ GeometryTestUtils.assertThrows(() -> {
+ AffineTransformMatrix3D.of(
+ 1, 0, 0, 0,
+ 0, Double.NEGATIVE_INFINITY, 0, 0,
+ 0, 0, 1, 0).getInverse();
+ }, NonInvertibleTransformException.class, "Transform is not invertible; matrix determinant is NaN");
+
+ GeometryTestUtils.assertThrows(() -> {
+ AffineTransformMatrix3D.of(
+ Double.POSITIVE_INFINITY, 0, 0, 0,
+ 0, 1, 0, 0,
+ 0, 0, 1, 0).getInverse();
+ }, NonInvertibleTransformException.class, "Transform is not invertible; matrix determinant is NaN");
+
+ GeometryTestUtils.assertThrows(() -> {
+ AffineTransformMatrix3D.of(
+ 1, 0, 0, Double.NaN,
+ 0, 1, 0, 0,
+ 0, 0, 1, 0).getInverse();
+ }, NonInvertibleTransformException.class, "Transform is not invertible; invalid matrix element: NaN");
+
+ GeometryTestUtils.assertThrows(() -> {
+ AffineTransformMatrix3D.of(
+ 1, 0, 0, 0,
+ 0, 1, 0, Double.POSITIVE_INFINITY,
+ 0, 0, 1, 0).getInverse();
+ }, NonInvertibleTransformException.class, "Transform is not invertible; invalid matrix element: Infinity");
+
+ GeometryTestUtils.assertThrows(() -> {
+ AffineTransformMatrix3D.of(
+ 1, 0, 0, 0,
+ 0, 1, 0, 0,
+ 0, 0, 1, Double.NEGATIVE_INFINITY).getInverse();
+ }, NonInvertibleTransformException.class, "Transform is not invertible; invalid matrix element: -Infinity");
+ }
+
+ @Test
+ public void testHashCode() {
+ // arrange
+ double[] values = new double[] {
+ 1, 2, 3, 4,
+ 5, 6, 7, 8,
+ 9, 10, 11, 12
+ };
+
+ // act/assert
+ int orig = AffineTransformMatrix3D.of(values).hashCode();
+ int same = AffineTransformMatrix3D.of(values).hashCode();
+
+ Assert.assertEquals(orig, same);
+
+ double[] temp;
+ for (int i=0; i<values.length; ++i) {
+ temp = values.clone();
+ temp[i] = 0;
+
+ int modified = AffineTransformMatrix3D.of(temp).hashCode();
+
+ Assert.assertNotEquals(orig, modified);
+ }
+ }
+
+ @Test
+ public void testEquals() {
+ // arrange
+ double[] values = new double[] {
+ 1, 2, 3, 4,
+ 5, 6, 7, 8,
+ 9, 10, 11, 12
+ };
+
+ AffineTransformMatrix3D a = AffineTransformMatrix3D.of(values);
+
+ // act/assert
+ Assert.assertTrue(a.equals(a));
+
+ Assert.assertFalse(a.equals(null));
+ Assert.assertFalse(a.equals(new Object()));
+
+ double[] temp;
+ for (int i=0; i<values.length; ++i) {
+ temp = values.clone();
+ temp[i] = 0;
+
+ AffineTransformMatrix3D modified = AffineTransformMatrix3D.of(temp);
+
+ Assert.assertFalse(a.equals(modified));
+ }
+ }
+
+ @Test
+ public void testToString() {
+ // arrange
+ AffineTransformMatrix3D a = AffineTransformMatrix3D.of(
+ 1, 2, 3, 4,
+ 5, 6, 7, 8,
+ 9, 10, 11, 12
+ );
+
+ // act
+ String result = a.toString();
+
+ // assert
+ Assert.assertEquals("[ 1.0, 2.0, 3.0, 4.0; "
+ + "5.0, 6.0, 7.0, 8.0; "
+ + "9.0, 10.0, 11.0, 12.0 ]", result);
+ }
+
+ /**
+ * Run the given test callback with a wide range of (x, y, z) inputs.
+ * @param test
+ */
+ private static void runWithCoordinates(PermuteCallback3D test) {
+ EuclideanTestUtils.permute(-1e-2, 1e-2, 5e-3, test);
+ EuclideanTestUtils.permute(-1e2, 1e2, 5, test);
+ }
+}
diff --git a/commons-geometry-euclidean/src/test/java/org/apache/commons/geometry/euclidean/threed/PlaneTest.java b/commons-geometry-euclidean/src/test/java/org/apache/commons/geometry/euclidean/threed/PlaneTest.java
index 92c6da1..b509024 100644
--- a/commons-geometry-euclidean/src/test/java/org/apache/commons/geometry/euclidean/threed/PlaneTest.java
+++ b/commons-geometry-euclidean/src/test/java/org/apache/commons/geometry/euclidean/threed/PlaneTest.java
@@ -16,10 +16,7 @@
*/
package org.apache.commons.geometry.euclidean.threed;
-import org.apache.commons.geometry.euclidean.threed.Line;
-import org.apache.commons.geometry.euclidean.threed.Plane;
-import org.apache.commons.geometry.euclidean.threed.Rotation;
-import org.apache.commons.geometry.euclidean.threed.Vector3D;
+import org.apache.commons.geometry.euclidean.threed.rotation.QuaternionRotation;
import org.junit.Assert;
import org.junit.Test;
@@ -72,17 +69,17 @@ public class PlaneTest {
Plane p = new Plane(p1, p2, p3, 1.0e-10);
Vector3D oldNormal = p.getNormal();
- p = p.rotate(p2, new Rotation(p2.subtract(p1), 1.7, RotationConvention.VECTOR_OPERATOR));
+ p = p.rotate(p2, QuaternionRotation.fromAxisAngle(p2.subtract(p1), 1.7));
Assert.assertTrue(p.contains(p1));
Assert.assertTrue(p.contains(p2));
Assert.assertTrue(! p.contains(p3));
- p = p.rotate(p2, new Rotation(oldNormal, 0.1, RotationConvention.VECTOR_OPERATOR));
+ p = p.rotate(p2, QuaternionRotation.fromAxisAngle(oldNormal, 0.1));
Assert.assertTrue(! p.contains(p1));
Assert.assertTrue(p.contains(p2));
Assert.assertTrue(! p.contains(p3));
- p = p.rotate(p1, new Rotation(oldNormal, 0.1, RotationConvention.VECTOR_OPERATOR));
+ p = p.rotate(p1, QuaternionRotation.fromAxisAngle(oldNormal, 0.1));
Assert.assertTrue(! p.contains(p1));
Assert.assertTrue(! p.contains(p2));
Assert.assertTrue(! p.contains(p3));
diff --git a/commons-geometry-euclidean/src/test/java/org/apache/commons/geometry/euclidean/threed/PolyhedronsSetTest.java b/commons-geometry-euclidean/src/test/java/org/apache/commons/geometry/euclidean/threed/PolyhedronsSetTest.java
index cd58e21..7af299c 100644
--- a/commons-geometry-euclidean/src/test/java/org/apache/commons/geometry/euclidean/threed/PolyhedronsSetTest.java
+++ b/commons-geometry-euclidean/src/test/java/org/apache/commons/geometry/euclidean/threed/PolyhedronsSetTest.java
@@ -33,6 +33,7 @@ import org.apache.commons.geometry.core.partitioning.Region;
import org.apache.commons.geometry.core.partitioning.RegionFactory;
import org.apache.commons.geometry.core.partitioning.SubHyperplane;
import org.apache.commons.geometry.euclidean.EuclideanTestUtils;
+import org.apache.commons.geometry.euclidean.threed.rotation.QuaternionRotation;
import org.apache.commons.geometry.euclidean.twod.PolygonsSet;
import org.apache.commons.geometry.euclidean.twod.SubLine;
import org.apache.commons.geometry.euclidean.twod.Vector2D;
@@ -543,14 +544,14 @@ public class PolyhedronsSetTest {
Vector3D barycenter = tree.getBarycenter();
Vector3D s = Vector3D.of(10.2, 4.3, -6.7);
Vector3D c = Vector3D.of(-0.2, 2.1, -3.2);
- Rotation r = new Rotation(Vector3D.of(6.2, -4.4, 2.1), 0.12, RotationConvention.VECTOR_OPERATOR);
+ QuaternionRotation r = QuaternionRotation.fromAxisAngle(Vector3D.of(6.2, -4.4, 2.1), 0.12);
tree = tree.rotate(c, r).translate(s);
Vector3D newB =
Vector3D.linearCombination(1.0, s,
1.0, c,
- 1.0, r.applyTo(barycenter.subtract(c)));
+ 1.0, r.apply(barycenter.subtract(c)));
Assert.assertEquals(0.0,
newB.subtract(tree.getBarycenter()).getNorm(),
TEST_TOLERANCE);
@@ -558,16 +559,16 @@ public class PolyhedronsSetTest {
final Vector3D[] expectedV = new Vector3D[] {
Vector3D.linearCombination(1.0, s,
1.0, c,
- 1.0, r.applyTo(vertex1.subtract(c))),
+ 1.0, r.apply(vertex1.subtract(c))),
Vector3D.linearCombination(1.0, s,
1.0, c,
- 1.0, r.applyTo(vertex2.subtract(c))),
+ 1.0, r.apply(vertex2.subtract(c))),
Vector3D.linearCombination(1.0, s,
1.0, c,
- 1.0, r.applyTo(vertex3.subtract(c))),
+ 1.0, r.apply(vertex3.subtract(c))),
Vector3D.linearCombination(1.0, s,
1.0, c,
- 1.0, r.applyTo(vertex4.subtract(c)))
+ 1.0, r.apply(vertex4.subtract(c)))
};
tree.getTree(true).visit(new BSPTreeVisitor<Vector3D>() {
diff --git a/commons-geometry-euclidean/src/test/java/org/apache/commons/geometry/euclidean/threed/RotationOrderTest.java b/commons-geometry-euclidean/src/test/java/org/apache/commons/geometry/euclidean/threed/RotationOrderTest.java
deleted file mode 100644
index 908cc6b..0000000
--- a/commons-geometry-euclidean/src/test/java/org/apache/commons/geometry/euclidean/threed/RotationOrderTest.java
+++ /dev/null
@@ -1,59 +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.geometry.euclidean.threed;
-
-import java.lang.reflect.Field;
-
-import org.apache.commons.geometry.euclidean.threed.RotationOrder;
-import org.junit.Assert;
-import org.junit.Test;
-
-
-public class RotationOrderTest {
-
- @Test
- public void testName() {
-
- RotationOrder[] orders = {
- RotationOrder.XYZ, RotationOrder.XZY, RotationOrder.YXZ,
- RotationOrder.YZX, RotationOrder.ZXY, RotationOrder.ZYX,
- RotationOrder.XYX, RotationOrder.XZX, RotationOrder.YXY,
- RotationOrder.YZY, RotationOrder.ZXZ, RotationOrder.ZYZ
- };
-
- for (int i = 0; i < orders.length; ++i) {
- Assert.assertEquals(getFieldName(orders[i]), orders[i].toString());
- }
-
- }
-
- private String getFieldName(RotationOrder order) {
- try {
- Field[] fields = RotationOrder.class.getFields();
- for (int i = 0; i < fields.length; ++i) {
- if (fields[i].get(null) == order) {
- return fields[i].getName();
- }
- }
- } catch (IllegalAccessException iae) {
- // ignored
- }
- return "unknown";
- }
-
-}
diff --git a/commons-geometry-euclidean/src/test/java/org/apache/commons/geometry/euclidean/threed/RotationTest.java b/commons-geometry-euclidean/src/test/java/org/apache/commons/geometry/euclidean/threed/RotationTest.java
deleted file mode 100644
index 136a4cb..0000000
--- a/commons-geometry-euclidean/src/test/java/org/apache/commons/geometry/euclidean/threed/RotationTest.java
+++ /dev/null
@@ -1,815 +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.geometry.euclidean.threed;
-
-import org.apache.commons.geometry.core.exception.IllegalNormException;
-import org.apache.commons.geometry.euclidean.threed.Rotation.CardanSingularityException;
-import org.apache.commons.geometry.euclidean.threed.Rotation.EulerSingularityException;
-import org.apache.commons.numbers.angle.PlaneAngleRadians;
-import org.junit.Assert;
-import org.junit.Test;
-
-
-public class RotationTest {
-
- @Test
- public void testIdentity() {
-
- Rotation r = Rotation.IDENTITY;
- checkVector(r.applyTo(Vector3D.PLUS_X), Vector3D.PLUS_X);
- checkVector(r.applyTo(Vector3D.PLUS_Y), Vector3D.PLUS_Y);
- checkVector(r.applyTo(Vector3D.PLUS_Z), Vector3D.PLUS_Z);
- checkAngle(r.getAngle(), 0);
-
- r = new Rotation(-1, 0, 0, 0, false);
- checkVector(r.applyTo(Vector3D.PLUS_X), Vector3D.PLUS_X);
- checkVector(r.applyTo(Vector3D.PLUS_Y), Vector3D.PLUS_Y);
- checkVector(r.applyTo(Vector3D.PLUS_Z), Vector3D.PLUS_Z);
- checkAngle(r.getAngle(), 0);
-
- r = new Rotation(42, 0, 0, 0, true);
- checkVector(r.applyTo(Vector3D.PLUS_X), Vector3D.PLUS_X);
- checkVector(r.applyTo(Vector3D.PLUS_Y), Vector3D.PLUS_Y);
- checkVector(r.applyTo(Vector3D.PLUS_Z), Vector3D.PLUS_Z);
- checkAngle(r.getAngle(), 0);
-
- }
-
- @Test
- @Deprecated
- public void testAxisAngleDeprecated() {
-
- Rotation r = new Rotation(Vector3D.of(10, 10, 10), 2 * Math.PI / 3);
- checkVector(r.applyTo(Vector3D.PLUS_X), Vector3D.PLUS_Y);
- checkVector(r.applyTo(Vector3D.PLUS_Y), Vector3D.PLUS_Z);
- checkVector(r.applyTo(Vector3D.PLUS_Z), Vector3D.PLUS_X);
- double s = 1 / Math.sqrt(3);
- checkVector(r.getAxis(), Vector3D.of(s, s, s));
- checkAngle(r.getAngle(), 2 * Math.PI / 3);
-
- try {
- new Rotation(Vector3D.of(0, 0, 0), 2 * Math.PI / 3);
- Assert.fail("an exception should have been thrown");
- } catch (IllegalNormException e) {
- }
-
- r = new Rotation(Vector3D.PLUS_Z, 1.5 * Math.PI);
- checkVector(r.getAxis(), Vector3D.of(0, 0, -1));
- checkAngle(r.getAngle(), 0.5 * Math.PI);
-
- r = new Rotation(Vector3D.PLUS_Y, Math.PI);
- checkVector(r.getAxis(), Vector3D.PLUS_Y);
- checkAngle(r.getAngle(), Math.PI);
-
- checkVector(Rotation.IDENTITY.getAxis(), Vector3D.PLUS_X);
-
- }
-
- @Test
- public void testAxisAngleVectorOperator() {
-
- Rotation r = new Rotation(Vector3D.of(10, 10, 10), 2 * Math.PI / 3, RotationConvention.VECTOR_OPERATOR);
- checkVector(r.applyTo(Vector3D.PLUS_X), Vector3D.PLUS_Y);
- checkVector(r.applyTo(Vector3D.PLUS_Y), Vector3D.PLUS_Z);
- checkVector(r.applyTo(Vector3D.PLUS_Z), Vector3D.PLUS_X);
- double s = 1 / Math.sqrt(3);
- checkVector(r.getAxis(RotationConvention.VECTOR_OPERATOR), Vector3D.of( s, s, s));
- checkVector(r.getAxis(RotationConvention.FRAME_TRANSFORM), Vector3D.of(-s, -s, -s));
- checkAngle(r.getAngle(), 2 * Math.PI / 3);
-
- try {
- new Rotation(Vector3D.of(0, 0, 0), 2 * Math.PI / 3, RotationConvention.VECTOR_OPERATOR);
- Assert.fail("an exception should have been thrown");
- } catch (IllegalNormException e) {
- }
-
- r = new Rotation(Vector3D.PLUS_Z, 1.5 * Math.PI, RotationConvention.VECTOR_OPERATOR);
- checkVector(r.getAxis(RotationConvention.VECTOR_OPERATOR), Vector3D.of(0, 0, -1));
- checkVector(r.getAxis(RotationConvention.FRAME_TRANSFORM), Vector3D.of(0, 0, +1));
- checkAngle(r.getAngle(), 0.5 * Math.PI);
-
- r = new Rotation(Vector3D.PLUS_Y, Math.PI, RotationConvention.VECTOR_OPERATOR);
- checkVector(r.getAxis(RotationConvention.VECTOR_OPERATOR), Vector3D.PLUS_Y);
- checkVector(r.getAxis(RotationConvention.FRAME_TRANSFORM), Vector3D.MINUS_Y);
- checkAngle(r.getAngle(), Math.PI);
-
- checkVector(Rotation.IDENTITY.getAxis(RotationConvention.VECTOR_OPERATOR), Vector3D.PLUS_X);
- checkVector(Rotation.IDENTITY.getAxis(RotationConvention.FRAME_TRANSFORM), Vector3D.MINUS_X);
-
- }
-
- @Test
- public void testAxisAngleFrameTransform() {
-
- Rotation r = new Rotation(Vector3D.of(10, 10, 10), 2 * Math.PI / 3, RotationConvention.FRAME_TRANSFORM);
- checkVector(r.applyTo(Vector3D.PLUS_X), Vector3D.PLUS_Z);
- checkVector(r.applyTo(Vector3D.PLUS_Y), Vector3D.PLUS_X);
- checkVector(r.applyTo(Vector3D.PLUS_Z), Vector3D.PLUS_Y);
- double s = 1 / Math.sqrt(3);
- checkVector(r.getAxis(RotationConvention.FRAME_TRANSFORM), Vector3D.of( s, s, s));
- checkVector(r.getAxis(RotationConvention.VECTOR_OPERATOR), Vector3D.of(-s, -s, -s));
- checkAngle(r.getAngle(), 2 * Math.PI / 3);
-
- try {
- new Rotation(Vector3D.of(0, 0, 0), 2 * Math.PI / 3, RotationConvention.FRAME_TRANSFORM);
- Assert.fail("an exception should have been thrown");
- } catch (IllegalNormException e) {
- }
-
- r = new Rotation(Vector3D.PLUS_Z, 1.5 * Math.PI, RotationConvention.FRAME_TRANSFORM);
- checkVector(r.getAxis(RotationConvention.FRAME_TRANSFORM), Vector3D.of(0, 0, -1));
- checkVector(r.getAxis(RotationConvention.VECTOR_OPERATOR), Vector3D.of(0, 0, +1));
- checkAngle(r.getAngle(), 0.5 * Math.PI);
-
- r = new Rotation(Vector3D.PLUS_Y, Math.PI, RotationConvention.FRAME_TRANSFORM);
- checkVector(r.getAxis(RotationConvention.FRAME_TRANSFORM), Vector3D.PLUS_Y);
- checkVector(r.getAxis(RotationConvention.VECTOR_OPERATOR), Vector3D.MINUS_Y);
- checkAngle(r.getAngle(), Math.PI);
-
- checkVector(Rotation.IDENTITY.getAxis(RotationConvention.FRAME_TRANSFORM), Vector3D.MINUS_X);
- checkVector(Rotation.IDENTITY.getAxis(RotationConvention.VECTOR_OPERATOR), Vector3D.PLUS_X);
-
- }
-
- @Test
- public void testRevertDeprecated() {
- Rotation r = new Rotation(0.001, 0.36, 0.48, 0.8, true);
- Rotation reverted = r.revert();
- checkRotation(r.applyTo(reverted), 1, 0, 0, 0);
- checkRotation(reverted.applyTo(r), 1, 0, 0, 0);
- Assert.assertEquals(r.getAngle(), reverted.getAngle(), 1.0e-12);
- Assert.assertEquals(-1,
- r.getAxis(RotationConvention.VECTOR_OPERATOR).dotProduct(
- reverted.getAxis(RotationConvention.VECTOR_OPERATOR)),
- 1.0e-12);
- }
-
- @Test
- public void testRevertVectorOperator() {
- Rotation r = new Rotation(0.001, 0.36, 0.48, 0.8, true);
- Rotation reverted = r.revert();
- checkRotation(r.compose(reverted, RotationConvention.VECTOR_OPERATOR), 1, 0, 0, 0);
- checkRotation(reverted.compose(r, RotationConvention.VECTOR_OPERATOR), 1, 0, 0, 0);
- Assert.assertEquals(r.getAngle(), reverted.getAngle(), 1.0e-12);
- Assert.assertEquals(-1,
- r.getAxis(RotationConvention.VECTOR_OPERATOR).dotProduct(
- reverted.getAxis(RotationConvention.VECTOR_OPERATOR)),
- 1.0e-12);
- }
-
- @Test
- public void testRevertFrameTransform() {
- Rotation r = new Rotation(0.001, 0.36, 0.48, 0.8, true);
- Rotation reverted = r.revert();
- checkRotation(r.compose(reverted, RotationConvention.FRAME_TRANSFORM), 1, 0, 0, 0);
- checkRotation(reverted.compose(r, RotationConvention.FRAME_TRANSFORM), 1, 0, 0, 0);
- Assert.assertEquals(r.getAngle(), reverted.getAngle(), 1.0e-12);
- Assert.assertEquals(-1,
- r.getAxis(RotationConvention.FRAME_TRANSFORM).dotProduct(
- reverted.getAxis(RotationConvention.FRAME_TRANSFORM)),
- 1.0e-12);
- }
-
- @Test
- public void testVectorOnePair() {
-
- Vector3D u = Vector3D.of(3, 2, 1);
- Vector3D v = Vector3D.of(-4, 2, 2);
- Rotation r = new Rotation(u, v);
- checkVector(r.applyTo(u.scalarMultiply(v.getNorm())), v.scalarMultiply(u.getNorm()));
-
- checkAngle(new Rotation(u, u.negate()).getAngle(), Math.PI);
-
- try {
- new Rotation(u, Vector3D.ZERO);
- Assert.fail("an exception should have been thrown");
- } catch (IllegalNormException e) {
- // expected behavior
- }
-
- }
-
- @Test
- public void testVectorTwoPairs() {
-
- Vector3D u1 = Vector3D.of(3, 0, 0);
- Vector3D u2 = Vector3D.of(0, 5, 0);
- Vector3D v1 = Vector3D.of(0, 0, 2);
- Vector3D v2 = Vector3D.of(-2, 0, 2);
- Rotation r = new Rotation(u1, u2, v1, v2);
- checkVector(r.applyTo(Vector3D.PLUS_X), Vector3D.PLUS_Z);
- checkVector(r.applyTo(Vector3D.PLUS_Y), Vector3D.MINUS_X);
-
- r = new Rotation(u1, u2, u1.negate(), u2.negate());
- Vector3D axis = r.getAxis(RotationConvention.VECTOR_OPERATOR);
- if (axis.dotProduct(Vector3D.PLUS_Z) > 0) {
- checkVector(axis, Vector3D.PLUS_Z);
- } else {
- checkVector(axis, Vector3D.MINUS_Z);
- }
- checkAngle(r.getAngle(), Math.PI);
-
- double sqrt = Math.sqrt(2) / 2;
- r = new Rotation(Vector3D.PLUS_X, Vector3D.PLUS_Y,
- Vector3D.of(0.5, 0.5, sqrt),
- Vector3D.of(0.5, 0.5, -sqrt));
- checkRotation(r, sqrt, 0.5, 0.5, 0);
-
- r = new Rotation(u1, u2, u1, u1.crossProduct(u2));
- checkRotation(r, sqrt, -sqrt, 0, 0);
-
- checkRotation(new Rotation(u1, u2, u1, u2), 1, 0, 0, 0);
-
- try {
- new Rotation(u1, u2, Vector3D.ZERO, v2);
- Assert.fail("an exception should have been thrown");
- } catch (IllegalNormException e) {
- // expected behavior
- }
-
- }
-
- @Test
- public void testMatrix() {
-
- try {
- new Rotation(new double[][] {
- { 0.0, 1.0, 0.0 },
- { 1.0, 0.0, 0.0 }
- }, 1.0e-7);
- Assert.fail("Expecting IllegalArgumentException");
- } catch (IllegalArgumentException nrme) {
- // expected behavior
- }
-
- try {
- new Rotation(new double[][] {
- { 0.445888, 0.797184, -0.407040 },
- { 0.821760, -0.184320, 0.539200 },
- { -0.354816, 0.574912, 0.737280 }
- }, 1.0e-7);
- Assert.fail("Expecting IllegalArgumentException");
- } catch (IllegalArgumentException nrme) {
- // expected behavior
- }
-
- try {
- new Rotation(new double[][] {
- { 0.4, 0.8, -0.4 },
- { -0.4, 0.6, 0.7 },
- { 0.8, -0.2, 0.5 }
- }, 1.0e-15);
- Assert.fail("Expecting IllegalArgumentException");
- } catch (IllegalArgumentException nrme) {
- // expected behavior
- }
-
- checkRotation(new Rotation(new double[][] {
- { 0.445888, 0.797184, -0.407040 },
- { -0.354816, 0.574912, 0.737280 },
- { 0.821760, -0.184320, 0.539200 }
- }, 1.0e-10),
- 0.8, 0.288, 0.384, 0.36);
-
- checkRotation(new Rotation(new double[][] {
- { 0.539200, 0.737280, 0.407040 },
- { 0.184320, -0.574912, 0.797184 },
- { 0.821760, -0.354816, -0.445888 }
- }, 1.0e-10),
- 0.36, 0.8, 0.288, 0.384);
-
- checkRotation(new Rotation(new double[][] {
- { -0.445888, 0.797184, -0.407040 },
- { 0.354816, 0.574912, 0.737280 },
- { 0.821760, 0.184320, -0.539200 }
- }, 1.0e-10),
- 0.384, 0.36, 0.8, 0.288);
-
- checkRotation(new Rotation(new double[][] {
- { -0.539200, 0.737280, 0.407040 },
- { -0.184320, -0.574912, 0.797184 },
- { 0.821760, 0.354816, 0.445888 }
- }, 1.0e-10),
- 0.288, 0.384, 0.36, 0.8);
-
- double[][] m1 = { { 0.0, 1.0, 0.0 },
- { 0.0, 0.0, 1.0 },
- { 1.0, 0.0, 0.0 } };
- Rotation r = new Rotation(m1, 1.0e-7);
- checkVector(r.applyTo(Vector3D.PLUS_X), Vector3D.PLUS_Z);
- checkVector(r.applyTo(Vector3D.PLUS_Y), Vector3D.PLUS_X);
- checkVector(r.applyTo(Vector3D.PLUS_Z), Vector3D.PLUS_Y);
-
- double[][] m2 = { { 0.83203, -0.55012, -0.07139 },
- { 0.48293, 0.78164, -0.39474 },
- { 0.27296, 0.29396, 0.91602 } };
- r = new Rotation(m2, 1.0e-12);
-
- double[][] m3 = r.getMatrix();
- double d00 = m2[0][0] - m3[0][0];
- double d01 = m2[0][1] - m3[0][1];
- double d02 = m2[0][2] - m3[0][2];
- double d10 = m2[1][0] - m3[1][0];
- double d11 = m2[1][1] - m3[1][1];
- double d12 = m2[1][2] - m3[1][2];
- double d20 = m2[2][0] - m3[2][0];
- double d21 = m2[2][1] - m3[2][1];
- double d22 = m2[2][2] - m3[2][2];
-
- Assert.assertTrue(Math.abs(d00) < 6.0e-6);
- Assert.assertTrue(Math.abs(d01) < 6.0e-6);
- Assert.assertTrue(Math.abs(d02) < 6.0e-6);
- Assert.assertTrue(Math.abs(d10) < 6.0e-6);
- Assert.assertTrue(Math.abs(d11) < 6.0e-6);
- Assert.assertTrue(Math.abs(d12) < 6.0e-6);
- Assert.assertTrue(Math.abs(d20) < 6.0e-6);
- Assert.assertTrue(Math.abs(d21) < 6.0e-6);
- Assert.assertTrue(Math.abs(d22) < 6.0e-6);
-
- Assert.assertTrue(Math.abs(d00) > 4.0e-7);
- Assert.assertTrue(Math.abs(d01) > 4.0e-7);
- Assert.assertTrue(Math.abs(d02) > 4.0e-7);
- Assert.assertTrue(Math.abs(d10) > 4.0e-7);
- Assert.assertTrue(Math.abs(d11) > 4.0e-7);
- Assert.assertTrue(Math.abs(d12) > 4.0e-7);
- Assert.assertTrue(Math.abs(d20) > 4.0e-7);
- Assert.assertTrue(Math.abs(d21) > 4.0e-7);
- Assert.assertTrue(Math.abs(d22) > 4.0e-7);
-
- for (int i = 0; i < 3; ++i) {
- for (int j = 0; j < 3; ++j) {
- double m3tm3 = m3[i][0] * m3[j][0]
- + m3[i][1] * m3[j][1]
- + m3[i][2] * m3[j][2];
- if (i == j) {
- Assert.assertTrue(Math.abs(m3tm3 - 1.0) < 1.0e-10);
- } else {
- Assert.assertTrue(Math.abs(m3tm3) < 1.0e-10);
- }
- }
- }
-
- checkVector(r.applyTo(Vector3D.PLUS_X),
- Vector3D.of(m3[0][0], m3[1][0], m3[2][0]));
- checkVector(r.applyTo(Vector3D.PLUS_Y),
- Vector3D.of(m3[0][1], m3[1][1], m3[2][1]));
- checkVector(r.applyTo(Vector3D.PLUS_Z),
- Vector3D.of(m3[0][2], m3[1][2], m3[2][2]));
-
- double[][] m4 = { { 1.0, 0.0, 0.0 },
- { 0.0, -1.0, 0.0 },
- { 0.0, 0.0, -1.0 } };
- r = new Rotation(m4, 1.0e-7);
- checkAngle(r.getAngle(), Math.PI);
-
- try {
- double[][] m5 = { { 0.0, 0.0, 1.0 },
- { 0.0, 1.0, 0.0 },
- { 1.0, 0.0, 0.0 } };
- r = new Rotation(m5, 1.0e-7);
- Assert.fail("got " + r + ", should have caught an exception");
- } catch (IllegalArgumentException e) {
- // expected
- }
-
- }
-
- @Test
- @Deprecated
- public void testAnglesDeprecated() {
-
- RotationOrder[] CardanOrders = {
- RotationOrder.XYZ, RotationOrder.XZY, RotationOrder.YXZ,
- RotationOrder.YZX, RotationOrder.ZXY, RotationOrder.ZYX
- };
-
- for (int i = 0; i < CardanOrders.length; ++i) {
- for (double alpha1 = 0.1; alpha1 < 6.2; alpha1 += 0.3) {
- for (double alpha2 = -1.55; alpha2 < 1.55; alpha2 += 0.3) {
- for (double alpha3 = 0.1; alpha3 < 6.2; alpha3 += 0.3) {
- Rotation r = new Rotation(CardanOrders[i], alpha1, alpha2, alpha3);
- double[] angles = r.getAngles(CardanOrders[i]);
- checkAngle(angles[0], alpha1);
- checkAngle(angles[1], alpha2);
- checkAngle(angles[2], alpha3);
- }
- }
- }
- }
-
- RotationOrder[] EulerOrders = {
- RotationOrder.XYX, RotationOrder.XZX, RotationOrder.YXY,
- RotationOrder.YZY, RotationOrder.ZXZ, RotationOrder.ZYZ
- };
-
- for (int i = 0; i < EulerOrders.length; ++i) {
- for (double alpha1 = 0.1; alpha1 < 6.2; alpha1 += 0.3) {
- for (double alpha2 = 0.05; alpha2 < 3.1; alpha2 += 0.3) {
- for (double alpha3 = 0.1; alpha3 < 6.2; alpha3 += 0.3) {
- Rotation r = new Rotation(EulerOrders[i],
- alpha1, alpha2, alpha3);
- double[] angles = r.getAngles(EulerOrders[i]);
- checkAngle(angles[0], alpha1);
- checkAngle(angles[1], alpha2);
- checkAngle(angles[2], alpha3);
- }
- }
- }
- }
-
- }
-
- @Test
- public void testAngles() {
-
- for (RotationConvention convention : RotationConvention.values()) {
- RotationOrder[] CardanOrders = {
- RotationOrder.XYZ, RotationOrder.XZY, RotationOrder.YXZ,
- RotationOrder.YZX, RotationOrder.ZXY, RotationOrder.ZYX
- };
-
- for (int i = 0; i < CardanOrders.length; ++i) {
- for (double alpha1 = 0.1; alpha1 < 6.2; alpha1 += 0.3) {
- for (double alpha2 = -1.55; alpha2 < 1.55; alpha2 += 0.3) {
- for (double alpha3 = 0.1; alpha3 < 6.2; alpha3 += 0.3) {
- Rotation r = new Rotation(CardanOrders[i], convention, alpha1, alpha2, alpha3);
- double[] angles = r.getAngles(CardanOrders[i], convention);
- checkAngle(angles[0], alpha1);
- checkAngle(angles[1], alpha2);
- checkAngle(angles[2], alpha3);
- }
- }
- }
- }
-
- RotationOrder[] EulerOrders = {
- RotationOrder.XYX, RotationOrder.XZX, RotationOrder.YXY,
- RotationOrder.YZY, RotationOrder.ZXZ, RotationOrder.ZYZ
- };
-
- for (int i = 0; i < EulerOrders.length; ++i) {
- for (double alpha1 = 0.1; alpha1 < 6.2; alpha1 += 0.3) {
- for (double alpha2 = 0.05; alpha2 < 3.1; alpha2 += 0.3) {
- for (double alpha3 = 0.1; alpha3 < 6.2; alpha3 += 0.3) {
- Rotation r = new Rotation(EulerOrders[i], convention,
- alpha1, alpha2, alpha3);
- double[] angles = r.getAngles(EulerOrders[i], convention);
- checkAngle(angles[0], alpha1);
- checkAngle(angles[1], alpha2);
- checkAngle(angles[2], alpha3);
- }
- }
- }
- }
- }
-
- }
-
- @Test
- public void testSingularities() {
-
- for (RotationConvention convention : RotationConvention.values()) {
- RotationOrder[] CardanOrders = {
- RotationOrder.XYZ, RotationOrder.XZY, RotationOrder.YXZ,
- RotationOrder.YZX, RotationOrder.ZXY, RotationOrder.ZYX
- };
-
- double[] singularCardanAngle = { Math.PI / 2, -Math.PI / 2 };
- for (int i = 0; i < CardanOrders.length; ++i) {
- for (int j = 0; j < singularCardanAngle.length; ++j) {
- Rotation r = new Rotation(CardanOrders[i], convention, 0.1, singularCardanAngle[j], 0.3);
- try {
- r.getAngles(CardanOrders[i], convention);
- Assert.fail("an exception should have been caught");
- } catch (CardanSingularityException cese) {
- // expected behavior
- }
- }
- }
-
- RotationOrder[] EulerOrders = {
- RotationOrder.XYX, RotationOrder.XZX, RotationOrder.YXY,
- RotationOrder.YZY, RotationOrder.ZXZ, RotationOrder.ZYZ
- };
-
- double[] singularEulerAngle = { 0, Math.PI };
- for (int i = 0; i < EulerOrders.length; ++i) {
- for (int j = 0; j < singularEulerAngle.length; ++j) {
- Rotation r = new Rotation(EulerOrders[i], convention, 0.1, singularEulerAngle[j], 0.3);
- try {
- r.getAngles(EulerOrders[i], convention);
- Assert.fail("an exception should have been caught");
- } catch (EulerSingularityException cese) {
- // expected behavior
- }
- }
- }
- }
-
-
- }
-
- @Test
- public void testQuaternion() {
-
- Rotation r1 = new Rotation(Vector3D.of(2, -3, 5), 1.7, RotationConvention.VECTOR_OPERATOR);
- double n = 23.5;
- Rotation r2 = new Rotation(n * r1.getQ0(), n * r1.getQ1(),
- n * r1.getQ2(), n * r1.getQ3(),
- true);
- for (double x = -0.9; x < 0.9; x += 0.2) {
- for (double y = -0.9; y < 0.9; y += 0.2) {
- for (double z = -0.9; z < 0.9; z += 0.2) {
- Vector3D u = Vector3D.of(x, y, z);
- checkVector(r2.applyTo(u), r1.applyTo(u));
- }
- }
- }
-
- r1 = new Rotation( 0.288, 0.384, 0.36, 0.8, false);
- checkRotation(r1, -r1.getQ0(), -r1.getQ1(), -r1.getQ2(), -r1.getQ3());
-
- }
-
- @Test
- public void testApplyTo() {
-
- Rotation r1 = new Rotation(Vector3D.of(2, -3, 5), 1.7, RotationConvention.VECTOR_OPERATOR);
- Rotation r2 = new Rotation(Vector3D.of(-1, 3, 2), 0.3, RotationConvention.VECTOR_OPERATOR);
- Rotation r3 = r2.applyTo(r1);
-
- for (double x = -0.9; x < 0.9; x += 0.2) {
- for (double y = -0.9; y < 0.9; y += 0.2) {
- for (double z = -0.9; z < 0.9; z += 0.2) {
- Vector3D u = Vector3D.of(x, y, z);
- checkVector(r2.applyTo(r1.applyTo(u)), r3.applyTo(u));
- }
- }
- }
-
- }
-
- @Test
- public void testComposeVectorOperator() {
-
- Rotation r1 = new Rotation(Vector3D.of(2, -3, 5), 1.7, RotationConvention.VECTOR_OPERATOR);
- Rotation r2 = new Rotation(Vector3D.of(-1, 3, 2), 0.3, RotationConvention.VECTOR_OPERATOR);
- Rotation r3 = r2.compose(r1, RotationConvention.VECTOR_OPERATOR);
-
- for (double x = -0.9; x < 0.9; x += 0.2) {
- for (double y = -0.9; y < 0.9; y += 0.2) {
- for (double z = -0.9; z < 0.9; z += 0.2) {
- Vector3D u = Vector3D.of(x, y, z);
- checkVector(r2.applyTo(r1.applyTo(u)), r3.applyTo(u));
- }
- }
- }
-
- }
-
- @Test
- public void testComposeFrameTransform() {
-
- Rotation r1 = new Rotation(Vector3D.of(2, -3, 5), 1.7, RotationConvention.FRAME_TRANSFORM);
- Rotation r2 = new Rotation(Vector3D.of(-1, 3, 2), 0.3, RotationConvention.FRAME_TRANSFORM);
- Rotation r3 = r2.compose(r1, RotationConvention.FRAME_TRANSFORM);
- Rotation r4 = r1.compose(r2, RotationConvention.VECTOR_OPERATOR);
- Assert.assertEquals(0.0, Rotation.distance(r3, r4), 1.0e-15);
-
- for (double x = -0.9; x < 0.9; x += 0.2) {
- for (double y = -0.9; y < 0.9; y += 0.2) {
- for (double z = -0.9; z < 0.9; z += 0.2) {
- Vector3D u = Vector3D.of(x, y, z);
- checkVector(r1.applyTo(r2.applyTo(u)), r3.applyTo(u));
- }
- }
- }
-
- }
-
- @Test
- public void testApplyInverseToRotation() {
-
- Rotation r1 = new Rotation(Vector3D.of(2, -3, 5), 1.7, RotationConvention.VECTOR_OPERATOR);
- Rotation r2 = new Rotation(Vector3D.of(-1, 3, 2), 0.3, RotationConvention.VECTOR_OPERATOR);
- Rotation r3 = r2.applyInverseTo(r1);
-
- for (double x = -0.9; x < 0.9; x += 0.2) {
- for (double y = -0.9; y < 0.9; y += 0.2) {
- for (double z = -0.9; z < 0.9; z += 0.2) {
- Vector3D u = Vector3D.of(x, y, z);
- checkVector(r2.applyInverseTo(r1.applyTo(u)), r3.applyTo(u));
- }
- }
- }
-
- }
-
- @Test
- public void testComposeInverseVectorOperator() {
-
- Rotation r1 = new Rotation(Vector3D.of(2, -3, 5), 1.7, RotationConvention.VECTOR_OPERATOR);
- Rotation r2 = new Rotation(Vector3D.of(-1, 3, 2), 0.3, RotationConvention.VECTOR_OPERATOR);
- Rotation r3 = r2.composeInverse(r1, RotationConvention.VECTOR_OPERATOR);
-
- for (double x = -0.9; x < 0.9; x += 0.2) {
- for (double y = -0.9; y < 0.9; y += 0.2) {
- for (double z = -0.9; z < 0.9; z += 0.2) {
- Vector3D u = Vector3D.of(x, y, z);
- checkVector(r2.applyInverseTo(r1.applyTo(u)), r3.applyTo(u));
- }
- }
- }
-
- }
-
- @Test
- public void testComposeInverseFrameTransform() {
-
- Rotation r1 = new Rotation(Vector3D.of(2, -3, 5), 1.7, RotationConvention.FRAME_TRANSFORM);
- Rotation r2 = new Rotation(Vector3D.of(-1, 3, 2), 0.3, RotationConvention.FRAME_TRANSFORM);
- Rotation r3 = r2.composeInverse(r1, RotationConvention.FRAME_TRANSFORM);
- Rotation r4 = r1.revert().composeInverse(r2.revert(), RotationConvention.VECTOR_OPERATOR);
- Assert.assertEquals(0.0, Rotation.distance(r3, r4), 1.0e-15);
-
- for (double x = -0.9; x < 0.9; x += 0.2) {
- for (double y = -0.9; y < 0.9; y += 0.2) {
- for (double z = -0.9; z < 0.9; z += 0.2) {
- Vector3D u = Vector3D.of(x, y, z);
- checkVector(r1.applyTo(r2.applyInverseTo(u)), r3.applyTo(u));
- }
- }
- }
-
- }
-
- @Test
- public void testArray() {
-
- Rotation r = new Rotation(Vector3D.of(2, -3, 5), 1.7, RotationConvention.VECTOR_OPERATOR);
-
- for (double x = -0.9; x < 0.9; x += 0.2) {
- for (double y = -0.9; y < 0.9; y += 0.2) {
- for (double z = -0.9; z < 0.9; z += 0.2) {
- Vector3D u = Vector3D.of(x, y, z);
- Vector3D v = r.applyTo(u);
- double[] inOut = new double[] { x, y, z };
- r.applyTo(inOut, inOut);
- Assert.assertEquals(v.getX(), inOut[0], 1.0e-10);
- Assert.assertEquals(v.getY(), inOut[1], 1.0e-10);
- Assert.assertEquals(v.getZ(), inOut[2], 1.0e-10);
- r.applyInverseTo(inOut, inOut);
- Assert.assertEquals(u.getX(), inOut[0], 1.0e-10);
- Assert.assertEquals(u.getY(), inOut[1], 1.0e-10);
- Assert.assertEquals(u.getZ(), inOut[2], 1.0e-10);
- }
- }
- }
-
- }
-
- @Test
- public void testApplyInverseTo() {
-
- Rotation r = new Rotation(Vector3D.of(2, -3, 5), 1.7, RotationConvention.VECTOR_OPERATOR);
- for (double lambda = 0; lambda < 6.2; lambda += 0.2) {
- for (double phi = -1.55; phi < 1.55; phi += 0.2) {
- Vector3D u = Vector3D.of(Math.cos(lambda) * Math.cos(phi),
- Math.sin(lambda) * Math.cos(phi),
- Math.sin(phi));
- r.applyInverseTo(r.applyTo(u));
- checkVector(u, r.applyInverseTo(r.applyTo(u)));
- checkVector(u, r.applyTo(r.applyInverseTo(u)));
- }
- }
-
- r = Rotation.IDENTITY;
- for (double lambda = 0; lambda < 6.2; lambda += 0.2) {
- for (double phi = -1.55; phi < 1.55; phi += 0.2) {
- Vector3D u = Vector3D.of(Math.cos(lambda) * Math.cos(phi),
- Math.sin(lambda) * Math.cos(phi),
- Math.sin(phi));
- checkVector(u, r.applyInverseTo(r.applyTo(u)));
- checkVector(u, r.applyTo(r.applyInverseTo(u)));
- }
- }
-
- r = new Rotation(Vector3D.PLUS_Z, Math.PI, RotationConvention.VECTOR_OPERATOR);
- for (double lambda = 0; lambda < 6.2; lambda += 0.2) {
- for (double phi = -1.55; phi < 1.55; phi += 0.2) {
- Vector3D u = Vector3D.of(Math.cos(lambda) * Math.cos(phi),
- Math.sin(lambda) * Math.cos(phi),
- Math.sin(phi));
- checkVector(u, r.applyInverseTo(r.applyTo(u)));
- checkVector(u, r.applyTo(r.applyInverseTo(u)));
- }
- }
-
- }
-
- @Test
- public void testIssue639() {
- Vector3D u1 = Vector3D.of(-1321008684645961.0 / 268435456.0,
- -5774608829631843.0 / 268435456.0,
- -3822921525525679.0 / 4294967296.0);
- Vector3D u2 =Vector3D.of( -5712344449280879.0 / 2097152.0,
- -2275058564560979.0 / 1048576.0,
- 4423475992255071.0 / 65536.0);
- Rotation rot = new Rotation(u1, u2, Vector3D.PLUS_X,Vector3D.PLUS_Z);
- Assert.assertEquals( 0.6228370359608200639829222, rot.getQ0(), 1.0e-15);
- Assert.assertEquals( 0.0257707621456498790029987, rot.getQ1(), 1.0e-15);
- Assert.assertEquals(-0.0000000002503012255839931, rot.getQ2(), 1.0e-15);
- Assert.assertEquals(-0.7819270390861109450724902, rot.getQ3(), 1.0e-15);
- }
-
- @Test
- public void testIssue801() {
- Vector3D u1 = Vector3D.of(0.9999988431610581, -0.0015210774290851095, 0.0);
- Vector3D u2 = Vector3D.of(0.0, 0.0, 1.0);
-
- Vector3D v1 = Vector3D.of(0.9999999999999999, 0.0, 0.0);
- Vector3D v2 = Vector3D.of(0.0, 0.0, -1.0);
-
- Rotation quat = new Rotation(u1, u2, v1, v2);
- double q2 = quat.getQ0() * quat.getQ0() +
- quat.getQ1() * quat.getQ1() +
- quat.getQ2() * quat.getQ2() +
- quat.getQ3() * quat.getQ3();
- Assert.assertEquals(1.0, q2, 1.0e-14);
- Assert.assertEquals(0.0, v1.angle(quat.applyTo(u1)), 1.0e-14);
- Assert.assertEquals(0.0, v2.angle(quat.applyTo(u2)), 1.0e-14);
-
- }
-
- @Test
- public void testGithubPullRequest22A() {
- final RotationOrder order = RotationOrder.ZYX;
- final double xRotation = Math.toDegrees(30);
- final double yRotation = Math.toDegrees(20);
- final double zRotation = Math.toDegrees(10);
- final Vector3D startingVector = Vector3D.PLUS_X;
- Vector3D appliedIndividually = startingVector;
- appliedIndividually = new Rotation(order, RotationConvention.FRAME_TRANSFORM, zRotation, 0, 0).applyTo(appliedIndividually);
- appliedIndividually = new Rotation(order, RotationConvention.FRAME_TRANSFORM, 0, yRotation, 0).applyTo(appliedIndividually);
- appliedIndividually = new Rotation(order, RotationConvention.FRAME_TRANSFORM, 0, 0, xRotation).applyTo(appliedIndividually);
-
- final Vector3D bad = new Rotation(order, RotationConvention.FRAME_TRANSFORM, zRotation, yRotation, xRotation).applyTo(startingVector);
-
- Assert.assertEquals(bad.getX(), appliedIndividually.getX(), 1e-12);
- Assert.assertEquals(bad.getY(), appliedIndividually.getY(), 1e-12);
- Assert.assertEquals(bad.getZ(), appliedIndividually.getZ(), 1e-12);
- }
-
- @Test
- public void testGithubPullRequest22B() {
- final RotationOrder order = RotationOrder.ZYX;
- final double xRotation = Math.toDegrees(30);
- final double yRotation = Math.toDegrees(20);
- final double zRotation = Math.toDegrees(10);
- final Vector3D startingVector = Vector3D.PLUS_X;
- Vector3D appliedIndividually = startingVector;
- appliedIndividually = new Rotation(order, RotationConvention.FRAME_TRANSFORM, zRotation, 0, 0).applyTo(appliedIndividually);
- appliedIndividually = new Rotation(order, RotationConvention.FRAME_TRANSFORM, 0, yRotation, 0).applyTo(appliedIndividually);
- appliedIndividually = new Rotation(order, RotationConvention.FRAME_TRANSFORM, 0, 0, xRotation).applyTo(appliedIndividually);
-
- final Rotation r1 = new Rotation(order.getA1(), zRotation, RotationConvention.FRAME_TRANSFORM);
- final Rotation r2 = new Rotation(order.getA2(), yRotation, RotationConvention.FRAME_TRANSFORM);
- final Rotation r3 = new Rotation(order.getA3(), xRotation, RotationConvention.FRAME_TRANSFORM);
- final Rotation composite = r1.compose(r2.compose(r3,
- RotationConvention.FRAME_TRANSFORM),
- RotationConvention.FRAME_TRANSFORM);
- final Vector3D good = composite.applyTo(startingVector);
-
- Assert.assertEquals(good.getX(), appliedIndividually.getX(), 1e-12);
- Assert.assertEquals(good.getY(), appliedIndividually.getY(), 1e-12);
- Assert.assertEquals(good.getZ(), appliedIndividually.getZ(), 1e-12);
- }
-
- private void checkVector(Vector3D v1, Vector3D v2) {
- Assert.assertTrue(v1.subtract(v2).getNorm() < 1.0e-10);
- }
-
- private void checkAngle(double a1, double a2) {
- Assert.assertEquals(a1, PlaneAngleRadians.normalize(a2, a1), 1.0e-10);
- }
-
- private void checkRotation(Rotation r, double q0, double q1, double q2, double q3) {
- Assert.assertEquals(0, Rotation.distance(r, new Rotation(q0, q1, q2, q3, false)), 1.0e-12);
- }
-
-}
diff --git a/commons-geometry-euclidean/src/test/java/org/apache/commons/geometry/euclidean/threed/Vector3DTest.java b/commons-geometry-euclidean/src/test/java/org/apache/commons/geometry/euclidean/threed/Vector3DTest.java
index 770659e..d4f78b8 100644
--- a/commons-geometry-euclidean/src/test/java/org/apache/commons/geometry/euclidean/threed/Vector3DTest.java
+++ b/commons-geometry-euclidean/src/test/java/org/apache/commons/geometry/euclidean/threed/Vector3DTest.java
@@ -919,6 +919,21 @@ public class Vector3DTest {
}
@Test
+ public void testTransform() {
+ // arrange
+ AffineTransformMatrix3D transform = AffineTransformMatrix3D.identity()
+ .scale(2)
+ .translate(1, 2, 3);
+
+ Vector3D v1 = Vector3D.of(1, 2, 3);
+ Vector3D v2 = Vector3D.of(-4, -5, -6);
+
+ // act/assert
+ checkVector(v1.transform(transform), 3, 6, 9);
+ checkVector(v2.transform(transform), -7, -8, -9);
+ }
+
+ @Test
public void testHashCode() {
// arrange
double delta = 10 * Precision.EPSILON;
diff --git a/commons-geometry-euclidean/src/test/java/org/apache/commons/geometry/euclidean/threed/rotation/AxisAngleSequenceTest.java b/commons-geometry-euclidean/src/test/java/org/apache/commons/geometry/euclidean/threed/rotation/AxisAngleSequenceTest.java
new file mode 100644
index 0000000..e470a7d
--- /dev/null
+++ b/commons-geometry-euclidean/src/test/java/org/apache/commons/geometry/euclidean/threed/rotation/AxisAngleSequenceTest.java
@@ -0,0 +1,124 @@
+/*
+ * 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.geometry.euclidean.threed.rotation;
+
+import org.junit.Assert;
+import org.junit.Test;
+
+public class AxisAngleSequenceTest {
+
+ @Test
+ public void testConstructor() {
+ // act
+ AxisAngleSequence seq = new AxisAngleSequence(AxisReferenceFrame.RELATIVE, AxisSequence.XYZ, 1, 2, 3);
+
+ // assert
+ Assert.assertEquals(AxisReferenceFrame.RELATIVE, seq.getReferenceFrame());
+ Assert.assertEquals(AxisSequence.XYZ, seq.getAxisSequence());
+ Assert.assertEquals(1, seq.getAngle1(), 0.0);
+ Assert.assertEquals(2, seq.getAngle2(), 0.0);
+ Assert.assertEquals(3, seq.getAngle3(), 0.0);
+ }
+
+ @Test
+ public void testGetAngles() {
+ // arrange
+ AxisAngleSequence seq = new AxisAngleSequence(AxisReferenceFrame.RELATIVE, AxisSequence.XYZ, 1, 2, 3);
+
+ // act
+ double[] angles = seq.getAngles();
+
+ // assert
+ Assert.assertArrayEquals(new double[] { 1, 2, 3 }, angles, 0.0);
+ }
+
+ @Test
+ public void testHashCode() {
+ // arrange
+ AxisAngleSequence seq = new AxisAngleSequence(AxisReferenceFrame.ABSOLUTE, AxisSequence.XYZ, 1, 2, 3);
+
+ // act/assert
+ Assert.assertNotEquals(seq.hashCode(), new AxisAngleSequence(AxisReferenceFrame.RELATIVE, AxisSequence.XYZ, 1, 2, 3).hashCode());
+ Assert.assertNotEquals(seq.hashCode(), new AxisAngleSequence(AxisReferenceFrame.ABSOLUTE, AxisSequence.ZYX, 1, 2, 3).hashCode());
+ Assert.assertNotEquals(seq.hashCode(), new AxisAngleSequence(AxisReferenceFrame.ABSOLUTE, AxisSequence.XYZ, 9, 2, 3).hashCode());
+ Assert.assertNotEquals(seq.hashCode(), new AxisAngleSequence(AxisReferenceFrame.ABSOLUTE, AxisSequence.XYZ, 1, 9, 3).hashCode());
+ Assert.assertNotEquals(seq.hashCode(), new AxisAngleSequence(AxisReferenceFrame.ABSOLUTE, AxisSequence.XYZ, 1, 2, 9).hashCode());
+
+ Assert.assertEquals(seq.hashCode(), new AxisAngleSequence(AxisReferenceFrame.ABSOLUTE, AxisSequence.XYZ, 1, 2, 3).hashCode());
+ }
+
+ @Test
+ public void testEquals() {
+ // arrange
+ AxisAngleSequence seq = new AxisAngleSequence(AxisReferenceFrame.ABSOLUTE, AxisSequence.XYZ, 1, 2, 3);
+
+ // act/assert
+ Assert.assertFalse(seq.equals(null));
+ Assert.assertFalse(seq.equals(new Object()));
+
+ Assert.assertFalse(seq.equals(new AxisAngleSequence(AxisReferenceFrame.RELATIVE, AxisSequence.XYZ, 1, 2, 3)));
+ Assert.assertFalse(seq.equals(new AxisAngleSequence(AxisReferenceFrame.ABSOLUTE, AxisSequence.ZYX, 1, 2, 3)));
+ Assert.assertFalse(seq.equals(new AxisAngleSequence(AxisReferenceFrame.ABSOLUTE, AxisSequence.XYZ, 9, 2, 3)));
+ Assert.assertFalse(seq.equals(new AxisAngleSequence(AxisReferenceFrame.ABSOLUTE, AxisSequence.XYZ, 1, 9, 3)));
+ Assert.assertFalse(seq.equals(new AxisAngleSequence(AxisReferenceFrame.ABSOLUTE, AxisSequence.XYZ, 1, 2, 9)));
+
+ Assert.assertTrue(seq.equals(seq));
+ Assert.assertTrue(seq.equals(new AxisAngleSequence(AxisReferenceFrame.ABSOLUTE, AxisSequence.XYZ, 1, 2, 3)));
+ }
+
+ @Test
+ public void testToString() {
+ // arrange
+ AxisAngleSequence seq = new AxisAngleSequence(AxisReferenceFrame.ABSOLUTE, AxisSequence.XYZ, 1, 2, 3);
+
+ // act
+ String str = seq.toString();
+
+ // assert
+ Assert.assertTrue(str.contains("ABSOLUTE"));
+ Assert.assertTrue(str.contains("XYZ"));
+ Assert.assertTrue(str.contains("1"));
+ Assert.assertTrue(str.contains("2"));
+ Assert.assertTrue(str.contains("3"));
+ }
+
+ @Test
+ public void testCreateRelative() {
+ // act
+ AxisAngleSequence seq = AxisAngleSequence.createRelative(AxisSequence.XYZ, 1, 2, 3);
+
+ // assert
+ Assert.assertEquals(AxisReferenceFrame.RELATIVE, seq.getReferenceFrame());
+ Assert.assertEquals(AxisSequence.XYZ, seq.getAxisSequence());
+ Assert.assertEquals(1, seq.getAngle1(), 0.0);
+ Assert.assertEquals(2, seq.getAngle2(), 0.0);
+ Assert.assertEquals(3, seq.getAngle3(), 0.0);
+ }
+
+ @Test
+ public void testCreateAbsolute() {
+ // act
+ AxisAngleSequence seq = AxisAngleSequence.createAbsolute(AxisSequence.XYZ, 1, 2, 3);
+
+ // assert
+ Assert.assertEquals(AxisReferenceFrame.ABSOLUTE, seq.getReferenceFrame());
+ Assert.assertEquals(AxisSequence.XYZ, seq.getAxisSequence());
+ Assert.assertEquals(1, seq.getAngle1(), 0.0);
+ Assert.assertEquals(2, seq.getAngle2(), 0.0);
+ Assert.assertEquals(3, seq.getAngle3(), 0.0);
+ }
+}
diff --git a/commons-geometry-euclidean/src/test/java/org/apache/commons/geometry/euclidean/threed/rotation/AxisSequenceTest.java b/commons-geometry-euclidean/src/test/java/org/apache/commons/geometry/euclidean/threed/rotation/AxisSequenceTest.java
new file mode 100644
index 0000000..4ef9e50
--- /dev/null
+++ b/commons-geometry-euclidean/src/test/java/org/apache/commons/geometry/euclidean/threed/rotation/AxisSequenceTest.java
@@ -0,0 +1,63 @@
+/*
+ * 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.geometry.euclidean.threed.rotation;
+
+import org.apache.commons.geometry.euclidean.threed.Vector3D;
+import org.apache.commons.geometry.euclidean.threed.rotation.AxisSequence;
+import org.junit.Assert;
+import org.junit.Test;
+
+public class AxisSequenceTest {
+
+ @Test
+ public void testAxes() {
+ // act/assert
+ for (AxisSequence axes : AxisSequence.values()) {
+ checkAxes(axes);
+ }
+ }
+
+ private void checkAxes(AxisSequence axes) {
+ // make sure that the name of the enum value matches
+ // the axes it contains
+ String name = axes.toString();
+
+ Vector3D a1 = getAxisForName(name.substring(0, 1));
+ Vector3D a2 = getAxisForName(name.substring(1, 2));
+ Vector3D a3 = getAxisForName(name.substring(2, 3));
+
+ // assert
+ Assert.assertEquals(a1, axes.getAxis1());
+ Assert.assertEquals(a2, axes.getAxis2());
+ Assert.assertEquals(a3, axes.getAxis3());
+
+ Assert.assertArrayEquals(new Vector3D[] { a1, a2, a3 }, axes.toArray());
+ }
+
+ private Vector3D getAxisForName(String name) {
+ if ("X".equals(name)) {
+ return Vector3D.PLUS_X;
+ }
+ if ("Y".equals(name)) {
+ return Vector3D.PLUS_Y;
+ }
+ if ("Z".equals(name)) {
+ return Vector3D.PLUS_Z;
+ }
+ throw new IllegalArgumentException("Unknown axis: " + name);
+ }
+}
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
new file mode 100644
index 0000000..c0e405b
--- /dev/null
+++ b/commons-geometry-euclidean/src/test/java/org/apache/commons/geometry/euclidean/threed/rotation/QuaterionRotationTest.java
@@ -0,0 +1,1613 @@
+/*
+ * 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.geometry.euclidean.threed.rotation;
+
+import java.util.Arrays;
+import java.util.List;
+import java.util.function.UnaryOperator;
+import java.util.regex.Pattern;
+import java.util.stream.Collectors;
+
+import org.apache.commons.geometry.core.Geometry;
+import org.apache.commons.geometry.core.GeometryTestUtils;
+import org.apache.commons.geometry.core.exception.IllegalNormException;
+import org.apache.commons.geometry.core.internal.SimpleTupleFormat;
+import org.apache.commons.geometry.euclidean.EuclideanTestUtils;
+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.geometry.euclidean.threed.rotation.AxisAngleSequence;
+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.junit.Assert;
+import org.junit.Test;
+
+public class QuaterionRotationTest {
+
+ private static final double EPS = 1e-12;
+
+ // use non-normalized axes to ensure that the axis is normalized
+ private static final Vector3D PLUS_X_DIR = Vector3D.of(2, 0, 0);
+ private static final Vector3D MINUS_X_DIR = Vector3D.of(-2, 0, 0);
+
+ private static final Vector3D PLUS_Y_DIR = Vector3D.of(0, 3, 0);
+ private static final Vector3D MINUS_Y_DIR = Vector3D.of(0, -3, 0);
+
+ private static final Vector3D PLUS_Z_DIR = Vector3D.of(0, 0, 4);
+ private static final Vector3D MINUS_Z_DIR = Vector3D.of(0, 0, -4);
+
+ private static final Vector3D PLUS_DIAGONAL = Vector3D.of(1, 1, 1);
+ private static final Vector3D MINUS_DIAGONAL = Vector3D.of(-1, -1, -1);
+
+ private static final double TWO_THIRDS_PI = 2.0 * Geometry.PI / 3.0;
+ private static final double MINUS_TWO_THIRDS_PI = -TWO_THIRDS_PI;
+
+ @Test
+ public void testOf() {
+ // act/assert
+ 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);
+ }
+
+ @Test
+ public void testOf_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);
+ }
+
+ @Test
+ public void testOf_arrayArg() {
+ // act/assert
+ 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);
+ }
+
+ @Test
+ public void testOf_arrayArg_illegalNorm() {
+ // act/assert
+ GeometryTestUtils.assertThrows(() -> QuaternionRotation.of(new double[] { 0, 0, 0, 0 }),
+ IllegalNormException.class);
+ GeometryTestUtils.assertThrows(() -> QuaternionRotation.of(new double[] { 1, 1, 1, Double.NaN }),
+ IllegalNormException.class);
+ GeometryTestUtils.assertThrows(() -> QuaternionRotation.of(new double[] { 1, 1, Double.POSITIVE_INFINITY, 1 }),
+ IllegalNormException.class);
+ GeometryTestUtils.assertThrows(() -> QuaternionRotation.of(new double[] { 1, Double.NEGATIVE_INFINITY, 1, 1 }),
+ IllegalNormException.class);
+ GeometryTestUtils.assertThrows(() -> QuaternionRotation.of(new double[] { Double.NaN, 1, 1, 1 }),
+ IllegalNormException.class);
+ }
+
+ @Test
+ public void testOf_arrayArg_invalidLength() {
+ // act/assert
+ GeometryTestUtils.assertThrows(() -> QuaternionRotation.of(new double[] { 1, 1, 1 }),
+ IllegalArgumentException.class, "Dimension mismatch: 3 != 4");
+ GeometryTestUtils.assertThrows(() -> QuaternionRotation.of(new double[] { 1, 1, 1, 1, 1 }),
+ IllegalArgumentException.class, "Dimension mismatch: 5 != 4");
+ }
+
+ @Test
+ public void testIdentity() {
+ // act
+ QuaternionRotation q = QuaternionRotation.identity();
+
+ // assert
+ assertRotationEquals(StandardRotations.IDENTITY, q);
+ }
+
+ @Test
+ public void testIdentity_axis() {
+ // arrange
+ QuaternionRotation q = QuaternionRotation.identity();
+
+ // act/assert
+ EuclideanTestUtils.assertCoordinatesEqual(Vector3D.PLUS_X, q.getAxis(), EPS);
+ }
+
+ @Test
+ public void testToArray() {
+ // arrange
+ QuaternionRotation q = QuaternionRotation.of(1, 2, 3, 4);
+ double base = 1.0 / Math.sqrt(30);
+
+ // act
+ double[] arr = q.toArray();
+
+ // assert
+ Assert.assertArrayEquals(new double[] {
+ base, 2 * base, 3 * base, 4 * base
+ }, arr, EPS);
+ }
+
+ @Test
+ 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(), 0, 1, 0);
+ checkVector(QuaternionRotation.of(0, -1, 0, 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);
+ }
+
+ @Test
+ public void testGetAxis_noAxis() {
+ // arrange
+ QuaternionRotation rot = QuaternionRotation.of(0, 0, 0, 1);
+
+ // act/assert
+ EuclideanTestUtils.assertCoordinatesEqual(Vector3D.PLUS_X, rot.getAxis(), EPS);
+ }
+
+ @Test
+ public void testGetAxis_matchesAxisAngleConstruction() {
+ EuclideanTestUtils.permuteSkipZero(-5, 5, 1, (x, y, z) -> {
+ // arrange
+ Vector3D vec = Vector3D.of(x, y, z);
+ Vector3D norm = vec.normalize();
+
+ // act/assert
+
+ // positive angle results in the axis being the normalized input axis
+ EuclideanTestUtils.assertCoordinatesEqual(norm,
+ QuaternionRotation.fromAxisAngle(vec, Geometry.HALF_PI).getAxis(), EPS);
+
+ // negative angle results in the axis being the negated normalized input axis
+ EuclideanTestUtils.assertCoordinatesEqual(norm,
+ QuaternionRotation.fromAxisAngle(vec.negate(), Geometry.MINUS_HALF_PI).getAxis(), EPS);
+ });
+ }
+
+ @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.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);
+ }
+
+ @Test
+ public void testGetAngle_matchesAxisAngleConstruction() {
+ for (double theta = -2 * Geometry.PI; theta <= 2 * Geometry.PI; theta += 0.1) {
+ // arrange
+ QuaternionRotation rot = QuaternionRotation.fromAxisAngle(PLUS_DIAGONAL, theta);
+
+ // act
+ double angle = rot.getAngle();
+
+ // assert
+ // make sure that we're in the [0, pi] range
+ Assert.assertTrue(angle >= Geometry.ZERO_PI);
+ Assert.assertTrue(angle <= Geometry.PI);
+
+ double expected = PlaneAngleRadians.normalizeBetweenMinusPiAndPi(theta);
+ if (PLUS_DIAGONAL.dotProduct(rot.getAxis()) < 0) {
+ // if the axis ended up being flipped, then negate the expected angle
+ expected *= -1;
+ }
+
+ Assert.assertEquals(expected, angle, EPS);
+ }
+ }
+
+ @Test
+ public void testFromAxisAngle_apply() {
+ // act/assert
+
+ // --- x axes
+ assertRotationEquals(StandardRotations.IDENTITY, QuaternionRotation.fromAxisAngle(PLUS_X_DIR, Geometry.ZERO_PI));
+
+ assertRotationEquals(StandardRotations.PLUS_X_HALF_PI, QuaternionRotation.fromAxisAngle(PLUS_X_DIR, Geometry.HALF_PI));
+ assertRotationEquals(StandardRotations.PLUS_X_HALF_PI, QuaternionRotation.fromAxisAngle(MINUS_X_DIR, Geometry.MINUS_HALF_PI));
+
+ assertRotationEquals(StandardRotations.MINUS_X_HALF_PI, QuaternionRotation.fromAxisAngle(MINUS_X_DIR, Geometry.HALF_PI));
+ assertRotationEquals(StandardRotations.MINUS_X_HALF_PI, QuaternionRotation.fromAxisAngle(PLUS_X_DIR, Geometry.MINUS_HALF_PI));
+
+ assertRotationEquals(StandardRotations.X_PI, QuaternionRotation.fromAxisAngle(PLUS_X_DIR, Geometry.PI));
+ assertRotationEquals(StandardRotations.X_PI, QuaternionRotation.fromAxisAngle(MINUS_X_DIR, Geometry.PI));
+
+ // --- y axes
+ assertRotationEquals(StandardRotations.IDENTITY, QuaternionRotation.fromAxisAngle(PLUS_Y_DIR, Geometry.ZERO_PI));
+
+ assertRotationEquals(StandardRotations.PLUS_Y_HALF_PI, QuaternionRotation.fromAxisAngle(PLUS_Y_DIR, Geometry.HALF_PI));
+ assertRotationEquals(StandardRotations.PLUS_Y_HALF_PI, QuaternionRotation.fromAxisAngle(MINUS_Y_DIR, Geometry.MINUS_HALF_PI));
+
+ assertRotationEquals(StandardRotations.MINUS_Y_HALF_PI, QuaternionRotation.fromAxisAngle(MINUS_Y_DIR, Geometry.HALF_PI));
+ assertRotationEquals(StandardRotations.MINUS_Y_HALF_PI, QuaternionRotation.fromAxisAngle(PLUS_Y_DIR, Geometry.MINUS_HALF_PI));
+
+ assertRotationEquals(StandardRotations.Y_PI, QuaternionRotation.fromAxisAngle(PLUS_Y_DIR, Geometry.PI));
+ assertRotationEquals(StandardRotations.Y_PI, QuaternionRotation.fromAxisAngle(MINUS_Y_DIR, Geometry.PI));
+
+ // --- z axes
+ assertRotationEquals(StandardRotations.IDENTITY, QuaternionRotation.fromAxisAngle(PLUS_Z_DIR, Geometry.ZERO_PI));
+
+ assertRotationEquals(StandardRotations.PLUS_Z_HALF_PI, QuaternionRotation.fromAxisAngle(PLUS_Z_DIR, Geometry.HALF_PI));
+ assertRotationEquals(StandardRotations.PLUS_Z_HALF_PI, QuaternionRotation.fromAxisAngle(MINUS_Z_DIR, Geometry.MINUS_HALF_PI));
+
+ assertRotationEquals(StandardRotations.MINUS_Z_HALF_PI, QuaternionRotation.fromAxisAngle(MINUS_Z_DIR, Geometry.HALF_PI));
+ assertRotationEquals(StandardRotations.MINUS_Z_HALF_PI, QuaternionRotation.fromAxisAngle(PLUS_Z_DIR, Geometry.MINUS_HALF_PI));
+
+ assertRotationEquals(StandardRotations.Z_PI, QuaternionRotation.fromAxisAngle(PLUS_Z_DIR, Geometry.PI));
+ assertRotationEquals(StandardRotations.Z_PI, QuaternionRotation.fromAxisAngle(MINUS_Z_DIR, Geometry.PI));
+
+ // --- diagonal
+ assertRotationEquals(StandardRotations.PLUS_DIAGONAL_TWO_THIRDS_PI, QuaternionRotation.fromAxisAngle(PLUS_DIAGONAL, TWO_THIRDS_PI));
+ assertRotationEquals(StandardRotations.PLUS_DIAGONAL_TWO_THIRDS_PI, QuaternionRotation.fromAxisAngle(MINUS_DIAGONAL, MINUS_TWO_THIRDS_PI));
+
+ assertRotationEquals(StandardRotations.MINUS_DIAGONAL_TWO_THIRDS_PI, QuaternionRotation.fromAxisAngle(MINUS_DIAGONAL, TWO_THIRDS_PI));
+ assertRotationEquals(StandardRotations.MINUS_DIAGONAL_TWO_THIRDS_PI, QuaternionRotation.fromAxisAngle(PLUS_DIAGONAL, MINUS_TWO_THIRDS_PI));
+ }
+
+ @Test
+ public void testFromAxisAngle_invalidAxisNorm() {
+ // act/assert
+ GeometryTestUtils.assertThrows(() -> QuaternionRotation.fromAxisAngle(Vector3D.ZERO, Geometry.HALF_PI), IllegalNormException.class);
+ GeometryTestUtils.assertThrows(() -> QuaternionRotation.fromAxisAngle(Vector3D.NaN, Geometry.HALF_PI), IllegalNormException.class);
+ GeometryTestUtils.assertThrows(() -> QuaternionRotation.fromAxisAngle(Vector3D.POSITIVE_INFINITY, Geometry.HALF_PI), IllegalNormException.class);
+ GeometryTestUtils.assertThrows(() -> QuaternionRotation.fromAxisAngle(Vector3D.NEGATIVE_INFINITY, Geometry.HALF_PI), IllegalNormException.class);
+ }
+
+ @Test
+ public void testFromAxisAngle_invalidAngle() {
+ // act/assert
+ GeometryTestUtils.assertThrows(() -> QuaternionRotation.fromAxisAngle(Vector3D.PLUS_X, Double.NaN), IllegalArgumentException.class,
+ "Invalid angle: NaN");
+ GeometryTestUtils.assertThrows(() -> QuaternionRotation.fromAxisAngle(Vector3D.PLUS_X, Double.POSITIVE_INFINITY), IllegalArgumentException.class,
+ "Invalid angle: Infinity");
+ GeometryTestUtils.assertThrows(() -> QuaternionRotation.fromAxisAngle(Vector3D.PLUS_X, Double.NEGATIVE_INFINITY), IllegalArgumentException.class,
+ "Invalid angle: -Infinity");
+ }
+
+ @Test
+ public void testGetInverse() {
+ // arrange
+ QuaternionRotation rot = QuaternionRotation.of(0.5, 0.5, 0.5, 0.5);
+
+ // act
+ QuaternionRotation neg = rot.getInverse();
+
+ // assert
+ Assert.assertEquals(-0.5, neg.getX(), EPS);
+ Assert.assertEquals(-0.5, neg.getY(), EPS);
+ Assert.assertEquals(-0.5, neg.getZ(), EPS);
+ Assert.assertEquals(0.5, neg.getW(), EPS);
+ }
+
+ @Test
+ public void testGetInverse_apply() {
+ // act/assert
+
+ // --- x axes
+ assertRotationEquals(StandardRotations.IDENTITY, QuaternionRotation.fromAxisAngle(PLUS_X_DIR, Geometry.ZERO_PI).getInverse());
+
+ assertRotationEquals(StandardRotations.PLUS_X_HALF_PI, QuaternionRotation.fromAxisAngle(PLUS_X_DIR, Geometry.MINUS_HALF_PI).getInverse());
+ assertRotationEquals(StandardRotations.PLUS_X_HALF_PI, QuaternionRotation.fromAxisAngle(MINUS_X_DIR, Geometry.HALF_PI).getInverse());
+
+ assertRotationEquals(StandardRotations.MINUS_X_HALF_PI, QuaternionRotation.fromAxisAngle(MINUS_X_DIR, Geometry.MINUS_HALF_PI).getInverse());
+ assertRotationEquals(StandardRotations.MINUS_X_HALF_PI, QuaternionRotation.fromAxisAngle(PLUS_X_DIR, Geometry.HALF_PI).getInverse());
+
+ assertRotationEquals(StandardRotations.X_PI, QuaternionRotation.fromAxisAngle(PLUS_X_DIR, Geometry.PI).getInverse());
+ assertRotationEquals(StandardRotations.X_PI, QuaternionRotation.fromAxisAngle(MINUS_X_DIR, Geometry.PI).getInverse());
+
+ // --- y axes
+ assertRotationEquals(StandardRotations.IDENTITY, QuaternionRotation.fromAxisAngle(PLUS_Y_DIR, Geometry.ZERO_PI).getInverse());
+
+ assertRotationEquals(StandardRotations.PLUS_Y_HALF_PI, QuaternionRotation.fromAxisAngle(PLUS_Y_DIR, Geometry.MINUS_HALF_PI).getInverse());
+ assertRotationEquals(StandardRotations.PLUS_Y_HALF_PI, QuaternionRotation.fromAxisAngle(MINUS_Y_DIR, Geometry.HALF_PI).getInverse());
+
+ assertRotationEquals(StandardRotations.MINUS_Y_HALF_PI, QuaternionRotation.fromAxisAngle(MINUS_Y_DIR, Geometry.MINUS_HALF_PI).getInverse());
+ assertRotationEquals(StandardRotations.MINUS_Y_HALF_PI, QuaternionRotation.fromAxisAngle(PLUS_Y_DIR, Geometry.HALF_PI).getInverse());
+
+ assertRotationEquals(StandardRotations.Y_PI, QuaternionRotation.fromAxisAngle(PLUS_Y_DIR, Geometry.PI).getInverse());
+ assertRotationEquals(StandardRotations.Y_PI, QuaternionRotation.fromAxisAngle(MINUS_Y_DIR, Geometry.PI).getInverse());
+
+ // --- z axes
+ assertRotationEquals(StandardRotations.IDENTITY, QuaternionRotation.fromAxisAngle(PLUS_Z_DIR, Geometry.ZERO_PI).getInverse());
+
+ assertRotationEquals(StandardRotations.PLUS_Z_HALF_PI, QuaternionRotation.fromAxisAngle(PLUS_Z_DIR, Geometry.MINUS_HALF_PI).getInverse());
+ assertRotationEquals(StandardRotations.PLUS_Z_HALF_PI, QuaternionRotation.fromAxisAngle(MINUS_Z_DIR, Geometry.HALF_PI).getInverse());
+
+ assertRotationEquals(StandardRotations.MINUS_Z_HALF_PI, QuaternionRotation.fromAxisAngle(MINUS_Z_DIR, Geometry.MINUS_HALF_PI).getInverse());
+ assertRotationEquals(StandardRotations.MINUS_Z_HALF_PI, QuaternionRotation.fromAxisAngle(PLUS_Z_DIR, Geometry.HALF_PI).getInverse());
+
+ assertRotationEquals(StandardRotations.Z_PI, QuaternionRotation.fromAxisAngle(PLUS_Z_DIR, Geometry.PI).getInverse());
+ assertRotationEquals(StandardRotations.Z_PI, QuaternionRotation.fromAxisAngle(MINUS_Z_DIR, Geometry.PI).getInverse());
+
+ // --- diagonal
+ assertRotationEquals(StandardRotations.PLUS_DIAGONAL_TWO_THIRDS_PI, QuaternionRotation.fromAxisAngle(PLUS_DIAGONAL, MINUS_TWO_THIRDS_PI).getInverse());
+ assertRotationEquals(StandardRotations.PLUS_DIAGONAL_TWO_THIRDS_PI, QuaternionRotation.fromAxisAngle(MINUS_DIAGONAL, TWO_THIRDS_PI).getInverse());
+
+ assertRotationEquals(StandardRotations.MINUS_DIAGONAL_TWO_THIRDS_PI, QuaternionRotation.fromAxisAngle(MINUS_DIAGONAL, MINUS_TWO_THIRDS_PI).getInverse());
+ assertRotationEquals(StandardRotations.MINUS_DIAGONAL_TWO_THIRDS_PI, QuaternionRotation.fromAxisAngle(PLUS_DIAGONAL, TWO_THIRDS_PI).getInverse());
+ }
+
+ @Test
+ public void testGetInverse_undoesOriginalRotation() {
+ EuclideanTestUtils.permuteSkipZero(-5, 5, 1, (x, y, z) -> {
+ // arrange
+ Vector3D vec = Vector3D.of(x, y, z);
+
+ QuaternionRotation rot = QuaternionRotation.fromAxisAngle(vec, 0.75 * Geometry.PI);
+ QuaternionRotation neg = rot.getInverse();
+
+ // act/assert
+ EuclideanTestUtils.assertCoordinatesEqual(PLUS_DIAGONAL, neg.apply(rot.apply(PLUS_DIAGONAL)), EPS);
+ EuclideanTestUtils.assertCoordinatesEqual(PLUS_DIAGONAL, rot.apply(neg.apply(PLUS_DIAGONAL)), EPS);
+ });
+ }
+
+ @Test
+ public void testMultiply_sameAxis_simple() {
+ // arrange
+ QuaternionRotation q1 = QuaternionRotation.fromAxisAngle(Vector3D.PLUS_X, 0.1 * Geometry.PI);
+ QuaternionRotation q2 = QuaternionRotation.fromAxisAngle(Vector3D.PLUS_X, 0.4 * Geometry.PI);
+
+ // act
+ QuaternionRotation result = q1.multiply(q2);
+
+ // assert
+ EuclideanTestUtils.assertCoordinatesEqual(Vector3D.PLUS_X, result.getAxis(), EPS);
+ Assert.assertEquals(Geometry.HALF_PI, result.getAngle(), EPS);
+
+ assertRotationEquals(StandardRotations.PLUS_X_HALF_PI, result);
+ }
+
+ @Test
+ public void testMultiply_sameAxis_multiple() {
+ // arrange
+ double oneThird = 1.0 / 3.0;
+ QuaternionRotation q1 = QuaternionRotation.fromAxisAngle(PLUS_DIAGONAL, 0.1 * Geometry.PI);
+ QuaternionRotation q2 = QuaternionRotation.fromAxisAngle(PLUS_DIAGONAL, oneThird * Geometry.PI);
+ QuaternionRotation q3 = QuaternionRotation.fromAxisAngle(MINUS_DIAGONAL, 0.4 * Geometry.PI);
+ QuaternionRotation q4 = QuaternionRotation.fromAxisAngle(PLUS_DIAGONAL, 0.3 * Geometry.PI);
+ QuaternionRotation q5 = QuaternionRotation.fromAxisAngle(MINUS_DIAGONAL, - oneThird * Geometry.PI);
+
+ // act
+ QuaternionRotation result = q1.multiply(q2).multiply(q3).multiply(q4).multiply(q5);
+
+ // assert
+ EuclideanTestUtils.assertCoordinatesEqual(PLUS_DIAGONAL.normalize(), result.getAxis(), EPS);
+ Assert.assertEquals(2.0 * Geometry.PI / 3.0, result.getAngle(), EPS);
+
+ assertRotationEquals(StandardRotations.PLUS_DIAGONAL_TWO_THIRDS_PI, result);
+ }
+
+ @Test
+ public void testMultiply_differentAxes() {
+ // arrange
+ QuaternionRotation q1 = QuaternionRotation.fromAxisAngle(Vector3D.PLUS_X, Geometry.HALF_PI);
+ QuaternionRotation q2 = QuaternionRotation.fromAxisAngle(Vector3D.PLUS_Y, Geometry.HALF_PI);
+
+ // act
+ QuaternionRotation result = q1.multiply(q2);
+
+ // assert
+ EuclideanTestUtils.assertCoordinatesEqual(PLUS_DIAGONAL.normalize(), result.getAxis(), EPS);
+ Assert.assertEquals(2.0 * Geometry.PI / 3.0, result.getAngle(), EPS);
+
+ assertRotationEquals(StandardRotations.PLUS_DIAGONAL_TWO_THIRDS_PI, result);
+
+ assertRotationEquals((v) -> {
+ Vector3D temp = StandardRotations.PLUS_Y_HALF_PI.apply(v);
+ return StandardRotations.PLUS_X_HALF_PI.apply(temp);
+ }, result);
+ }
+
+ @Test
+ public void testMultiply_orderOfOperations() {
+ // arrange
+ QuaternionRotation q1 = QuaternionRotation.fromAxisAngle(Vector3D.PLUS_X, Geometry.HALF_PI);
+ QuaternionRotation q2 = QuaternionRotation.fromAxisAngle(Vector3D.PLUS_Y, Geometry.PI);
+ QuaternionRotation q3 = QuaternionRotation.fromAxisAngle(Vector3D.MINUS_Z, Geometry.HALF_PI);
+
+ // act
+ QuaternionRotation result = q3.multiply(q2).multiply(q1);
+
+ // assert
+ assertRotationEquals((v) -> {
+ Vector3D temp = StandardRotations.PLUS_X_HALF_PI.apply(v);
+ temp = StandardRotations.Y_PI.apply(temp);
+ return StandardRotations.MINUS_Z_HALF_PI.apply(temp);
+ }, result);
+ }
+
+ @Test
+ public void testPremultiply_sameAxis_simple() {
+ // arrange
+ QuaternionRotation q1 = QuaternionRotation.fromAxisAngle(Vector3D.PLUS_X, 0.1 * Geometry.PI);
+ QuaternionRotation q2 = QuaternionRotation.fromAxisAngle(Vector3D.PLUS_X, 0.4 * Geometry.PI);
+
+ // act
+ QuaternionRotation result = q1.premultiply(q2);
+
+ // assert
+ EuclideanTestUtils.assertCoordinatesEqual(Vector3D.PLUS_X, result.getAxis(), EPS);
+ Assert.assertEquals(Geometry.HALF_PI, result.getAngle(), EPS);
+
+ assertRotationEquals(StandardRotations.PLUS_X_HALF_PI, result);
+ }
+
+ @Test
+ public void testPremultiply_sameAxis_multiple() {
+ // arrange
+ double oneThird = 1.0 / 3.0;
+ QuaternionRotation q1 = QuaternionRotation.fromAxisAngle(PLUS_DIAGONAL, 0.1 * Geometry.PI);
+ QuaternionRotation q2 = QuaternionRotation.fromAxisAngle(PLUS_DIAGONAL, oneThird * Geometry.PI);
+ QuaternionRotation q3 = QuaternionRotation.fromAxisAngle(MINUS_DIAGONAL, 0.4 * Geometry.PI);
+ QuaternionRotation q4 = QuaternionRotation.fromAxisAngle(PLUS_DIAGONAL, 0.3 * Geometry.PI);
+ QuaternionRotation q5 = QuaternionRotation.fromAxisAngle(MINUS_DIAGONAL, - oneThird * Geometry.PI);
+
+ // act
+ QuaternionRotation result = q1.premultiply(q2).premultiply(q3).premultiply(q4).premultiply(q5);
+
+ // assert
+ EuclideanTestUtils.assertCoordinatesEqual(PLUS_DIAGONAL.normalize(), result.getAxis(), EPS);
+ Assert.assertEquals(2.0 * Geometry.PI / 3.0, result.getAngle(), EPS);
+
+ assertRotationEquals(StandardRotations.PLUS_DIAGONAL_TWO_THIRDS_PI, result);
+ }
+
+ @Test
+ public void testPremultiply_differentAxes() {
+ // arrange
+ QuaternionRotation q1 = QuaternionRotation.fromAxisAngle(Vector3D.PLUS_X, Geometry.HALF_PI);
+ QuaternionRotation q2 = QuaternionRotation.fromAxisAngle(Vector3D.PLUS_Y, Geometry.HALF_PI);
+
+ // act
+ QuaternionRotation result = q2.premultiply(q1);
+
+ // assert
+ EuclideanTestUtils.assertCoordinatesEqual(PLUS_DIAGONAL.normalize(), result.getAxis(), EPS);
+ Assert.assertEquals(2.0 * Geometry.PI / 3.0, result.getAngle(), EPS);
+
+ assertRotationEquals(StandardRotations.PLUS_DIAGONAL_TWO_THIRDS_PI, result);
+
+ assertRotationEquals((v) -> {
+ Vector3D temp = StandardRotations.PLUS_Y_HALF_PI.apply(v);
+ return StandardRotations.PLUS_X_HALF_PI.apply(temp);
+ }, result);
+ }
+
+ @Test
+ public void testPremultiply_orderOfOperations() {
+ // arrange
+ QuaternionRotation q1 = QuaternionRotation.fromAxisAngle(Vector3D.PLUS_X, Geometry.HALF_PI);
+ QuaternionRotation q2 = QuaternionRotation.fromAxisAngle(Vector3D.PLUS_Y, Geometry.PI);
+ QuaternionRotation q3 = QuaternionRotation.fromAxisAngle(Vector3D.MINUS_Z, Geometry.HALF_PI);
+
+ // act
+ QuaternionRotation result = q1.premultiply(q2).premultiply(q3);
+
+ // assert
+ assertRotationEquals((v) -> {
+ Vector3D temp = StandardRotations.PLUS_X_HALF_PI.apply(v);
+ temp = StandardRotations.Y_PI.apply(temp);
+ return StandardRotations.MINUS_Z_HALF_PI.apply(temp);
+ }, result);
+ }
+
+ @Test
+ public void testSlerp_simple() {
+ // arrange
+ QuaternionRotation q0 = QuaternionRotation.fromAxisAngle(Vector3D.PLUS_Z, Geometry.ZERO_PI);
+ QuaternionRotation q1 = QuaternionRotation.fromAxisAngle(Vector3D.PLUS_Z, Geometry.PI);
+
+ Vector3D v = Vector3D.of(2, 0, 1);
+
+ double sqrt2 = Math.sqrt(2);
+
+ // act
+ checkVector(q0.slerp(q1, 0).apply(v), 2, 0, 1);
+ checkVector(q0.slerp(q1, 0.25).apply(v), sqrt2, sqrt2, 1);
+ checkVector(q0.slerp(q1, 0.5).apply(v), 0, 2, 1);
+ checkVector(q0.slerp(q1, 0.75).apply(v), -sqrt2, sqrt2, 1);
+ checkVector(q0.slerp(q1, 1).apply(v), -2, 0, 1);
+ }
+
+ @Test
+ public void testSlerp_multipleCombinations() {
+ // arrange
+ QuaternionRotation[] rotations = {
+ QuaternionRotation.fromAxisAngle(Vector3D.PLUS_X, Geometry.ZERO_PI),
+ QuaternionRotation.fromAxisAngle(Vector3D.PLUS_X, Geometry.HALF_PI),
+ QuaternionRotation.fromAxisAngle(Vector3D.PLUS_X, Geometry.PI),
+
+ QuaternionRotation.fromAxisAngle(Vector3D.MINUS_X, Geometry.ZERO_PI),
+ QuaternionRotation.fromAxisAngle(Vector3D.MINUS_X, Geometry.HALF_PI),
+ QuaternionRotation.fromAxisAngle(Vector3D.MINUS_X, Geometry.PI),
+
+ QuaternionRotation.fromAxisAngle(Vector3D.PLUS_Y, Geometry.ZERO_PI),
+ QuaternionRotation.fromAxisAngle(Vector3D.PLUS_Y, Geometry.HALF_PI),
+ QuaternionRotation.fromAxisAngle(Vector3D.PLUS_Y, Geometry.PI),
+
+ QuaternionRotation.fromAxisAngle(Vector3D.MINUS_Y, Geometry.ZERO_PI),
+ QuaternionRotation.fromAxisAngle(Vector3D.MINUS_Y, Geometry.HALF_PI),
+ QuaternionRotation.fromAxisAngle(Vector3D.MINUS_Y, Geometry.PI),
+
+ QuaternionRotation.fromAxisAngle(Vector3D.PLUS_Z, Geometry.ZERO_PI),
+ QuaternionRotation.fromAxisAngle(Vector3D.PLUS_Z, Geometry.HALF_PI),
+ QuaternionRotation.fromAxisAngle(Vector3D.PLUS_Z, Geometry.PI),
+
+ QuaternionRotation.fromAxisAngle(Vector3D.MINUS_Z, Geometry.ZERO_PI),
+ QuaternionRotation.fromAxisAngle(Vector3D.MINUS_Z, Geometry.HALF_PI),
+ QuaternionRotation.fromAxisAngle(Vector3D.MINUS_Z, Geometry.PI),
+ };
+
+ // act/assert
+ // test each rotation against all of the others (including itself)
+ for (int i=0; i<rotations.length; ++i) {
+ for (int j=0; j<rotations.length; ++j) {
+ checkSlerpCombination(rotations[i], rotations[j]);
+ }
+ }
+ }
+
+ private void checkSlerpCombination(QuaternionRotation start, QuaternionRotation end) {
+ Vector3D vec = Vector3D.of(1, 1, 1).normalize();
+
+ Vector3D startVec = start.apply(vec);
+ Vector3D endVec = end.apply(vec);
+
+ // check start and end values
+ EuclideanTestUtils.assertCoordinatesEqual(startVec, start.slerp(end, 0.0).apply(vec), EPS);
+ EuclideanTestUtils.assertCoordinatesEqual(endVec, start.slerp(end, 1.0).apply(vec), EPS);
+
+ // check intermediate values
+ double prevAngle = -1.0;
+ for (double t=0; t<=1.0; t+= 0.01) {
+ QuaternionRotation result = start.slerp(end, t);
+
+ Vector3D slerpVec = result.apply(vec);
+ Assert.assertEquals(1.0, slerpVec.getNorm(), EPS);
+
+ // make sure that we're steadily progressing to the end angle
+ double angle = slerpVec.angle(startVec);
+ Assert.assertTrue("Expected slerp angle to continuously increase; previous angle was " +
+ prevAngle + " and new angle is " + angle, Precision.compareTo(angle, prevAngle, EPS) >= 0);
+
+ prevAngle = angle;
+ }
+ }
+
+ @Test
+ public void testSlerp_followsShortestPath() {
+ // arrange
+ QuaternionRotation q1 = QuaternionRotation.fromAxisAngle(Vector3D.PLUS_Z, 0.75 * Geometry.PI);
+ QuaternionRotation q2 = QuaternionRotation.fromAxisAngle(Vector3D.PLUS_Z, - 0.75 * Geometry.PI);
+
+ // act
+ QuaternionRotation result = q1.slerp(q2, 0.5);
+
+ // assert
+ // the slerp should have followed the path around the pi coordinate of the circle rather than
+ // the one through the zero coordinate
+ EuclideanTestUtils.assertCoordinatesEqual(Vector3D.MINUS_X, result.apply(Vector3D.PLUS_X), EPS);
+
+ EuclideanTestUtils.assertCoordinatesEqual(Vector3D.PLUS_Z, result.getAxis(), EPS);
+ Assert.assertEquals(Geometry.PI, result.getAngle(), EPS);
+ }
+
+ @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
+
+ // act
+ QuaternionRotation result = q1.slerp(q2, 0.5);
+
+ // assert
+ EuclideanTestUtils.assertCoordinatesEqual(Vector3D.PLUS_Y, result.apply(Vector3D.PLUS_X), EPS);
+
+ Assert.assertEquals(Geometry.HALF_PI, result.getAngle(), EPS);
+ EuclideanTestUtils.assertCoordinatesEqual(Vector3D.PLUS_Z, result.getAxis(), EPS);
+ }
+
+ @Test
+ public void testSlerp_quaternionIsNormalizedForAllT() {
+ // arrange
+ QuaternionRotation q1 = QuaternionRotation.fromAxisAngle(Vector3D.PLUS_Z, 0.25 * Geometry.PI);
+ QuaternionRotation q2 = QuaternionRotation.fromAxisAngle(Vector3D.PLUS_Z, 0.75 * Geometry.PI);
+
+ for (double t=-10.0; t<=10.0; t+=0.1) {
+ // act
+ QuaternionRotation result = q1.slerp(q2, t);
+
+ // assert
+ Assert.assertEquals(1.0, Vectors.norm(result.getX(), result.getY(), result.getZ(), result.getW()), EPS);
+ }
+ }
+
+ @Test
+ public void testSlerp_tOutsideOfZeroToOne_apply() {
+ // arrange
+ Vector3D vec = Vector3D.PLUS_X;
+
+ QuaternionRotation q1 = QuaternionRotation.fromAxisAngle(Vector3D.PLUS_Z, 0.25 * Geometry.PI);
+ QuaternionRotation q2 = QuaternionRotation.fromAxisAngle(Vector3D.PLUS_Z, 0.75 * Geometry.PI);
+
+ // act/assert
+ EuclideanTestUtils.assertCoordinatesEqual(Vector3D.PLUS_X, q1.slerp(q2, -4.5).apply(vec), EPS);
+ EuclideanTestUtils.assertCoordinatesEqual(Vector3D.PLUS_X, q1.slerp(q2, -0.5).apply(vec), EPS);
+ EuclideanTestUtils.assertCoordinatesEqual(Vector3D.MINUS_X, q1.slerp(q2, 1.5).apply(vec), EPS);
+ EuclideanTestUtils.assertCoordinatesEqual(Vector3D.MINUS_X, q1.slerp(q2, 5.5).apply(vec), EPS);
+
+ EuclideanTestUtils.assertCoordinatesEqual(Vector3D.MINUS_X, q2.slerp(q1, -4.5).apply(vec), EPS);
+ EuclideanTestUtils.assertCoordinatesEqual(Vector3D.MINUS_X, q2.slerp(q1, -0.5).apply(vec), EPS);
+ EuclideanTestUtils.assertCoordinatesEqual(Vector3D.PLUS_X, q2.slerp(q1, 1.5).apply(vec), EPS);
+ EuclideanTestUtils.assertCoordinatesEqual(Vector3D.PLUS_X, q2.slerp(q1, 5.5).apply(vec), EPS);
+ }
+
+ @Test
+ public void testToTransformMatrix() {
+ // act/assert
+ // --- x axes
+ assertTransformEquals(StandardRotations.IDENTITY, QuaternionRotation.fromAxisAngle(PLUS_X_DIR, Geometry.ZERO_PI).toTransformMatrix());
+
+ assertTransformEquals(StandardRotations.PLUS_X_HALF_PI, QuaternionRotation.fromAxisAngle(PLUS_X_DIR, Geometry.HALF_PI).toTransformMatrix());
+ assertTransformEquals(StandardRotations.PLUS_X_HALF_PI, QuaternionRotation.fromAxisAngle(MINUS_X_DIR, Geometry.MINUS_HALF_PI).toTransformMatrix());
+
+ assertTransformEquals(StandardRotations.MINUS_X_HALF_PI, QuaternionRotation.fromAxisAngle(MINUS_X_DIR, Geometry.HALF_PI).toTransformMatrix());
+ assertTransformEquals(StandardRotations.MINUS_X_HALF_PI, QuaternionRotation.fromAxisAngle(PLUS_X_DIR, Geometry.MINUS_HALF_PI).toTransformMatrix());
+
+ assertTransformEquals(StandardRotations.X_PI, QuaternionRotation.fromAxisAngle(PLUS_X_DIR, Geometry.PI).toTransformMatrix());
+ assertTransformEquals(StandardRotations.X_PI, QuaternionRotation.fromAxisAngle(MINUS_X_DIR, Geometry.PI).toTransformMatrix());
+
+ // --- y axes
+ assertTransformEquals(StandardRotations.IDENTITY, QuaternionRotation.fromAxisAngle(PLUS_Y_DIR, Geometry.ZERO_PI).toTransformMatrix());
+
+ assertTransformEquals(StandardRotations.PLUS_Y_HALF_PI, QuaternionRotation.fromAxisAngle(PLUS_Y_DIR, Geometry.HALF_PI).toTransformMatrix());
+ assertTransformEquals(StandardRotations.PLUS_Y_HALF_PI, QuaternionRotation.fromAxisAngle(MINUS_Y_DIR, Geometry.MINUS_HALF_PI).toTransformMatrix());
+
+ assertTransformEquals(StandardRotations.MINUS_Y_HALF_PI, QuaternionRotation.fromAxisAngle(MINUS_Y_DIR, Geometry.HALF_PI).toTransformMatrix());
+ assertTransformEquals(StandardRotations.MINUS_Y_HALF_PI, QuaternionRotation.fromAxisAngle(PLUS_Y_DIR, Geometry.MINUS_HALF_PI).toTransformMatrix());
+
+ assertTransformEquals(StandardRotations.Y_PI, QuaternionRotation.fromAxisAngle(PLUS_Y_DIR, Geometry.PI).toTransformMatrix());
+ assertTransformEquals(StandardRotations.Y_PI, QuaternionRotation.fromAxisAngle(MINUS_Y_DIR, Geometry.PI).toTransformMatrix());
+
+ // --- z axes
+ assertTransformEquals(StandardRotations.IDENTITY, QuaternionRotation.fromAxisAngle(PLUS_Z_DIR, Geometry.ZERO_PI).toTransformMatrix());
+
+ assertTransformEquals(StandardRotations.PLUS_Z_HALF_PI, QuaternionRotation.fromAxisAngle(PLUS_Z_DIR, Geometry.HALF_PI).toTransformMatrix());
+ assertTransformEquals(StandardRotations.PLUS_Z_HALF_PI, QuaternionRotation.fromAxisAngle(MINUS_Z_DIR, Geometry.MINUS_HALF_PI).toTransformMatrix());
+
+ assertTransformEquals(StandardRotations.MINUS_Z_HALF_PI, QuaternionRotation.fromAxisAngle(MINUS_Z_DIR, Geometry.HALF_PI).toTransformMatrix());
+ assertTransformEquals(StandardRotations.MINUS_Z_HALF_PI, QuaternionRotation.fromAxisAngle(PLUS_Z_DIR, Geometry.MINUS_HALF_PI).toTransformMatrix());
+
+ assertTransformEquals(StandardRotations.Z_PI, QuaternionRotation.fromAxisAngle(PLUS_Z_DIR, Geometry.PI).toTransformMatrix());
+ assertTransformEquals(StandardRotations.Z_PI, QuaternionRotation.fromAxisAngle(MINUS_Z_DIR, Geometry.PI).toTransformMatrix());
+
+ // --- diagonal
+ assertTransformEquals(StandardRotations.PLUS_DIAGONAL_TWO_THIRDS_PI, QuaternionRotation.fromAxisAngle(PLUS_DIAGONAL, TWO_THIRDS_PI).toTransformMatrix());
+ assertTransformEquals(StandardRotations.PLUS_DIAGONAL_TWO_THIRDS_PI, QuaternionRotation.fromAxisAngle(MINUS_DIAGONAL, MINUS_TWO_THIRDS_PI).toTransformMatrix());
+
+ assertTransformEquals(StandardRotations.MINUS_DIAGONAL_TWO_THIRDS_PI, QuaternionRotation.fromAxisAngle(MINUS_DIAGONAL, TWO_THIRDS_PI).toTransformMatrix());
+ assertTransformEquals(StandardRotations.MINUS_DIAGONAL_TWO_THIRDS_PI, QuaternionRotation.fromAxisAngle(PLUS_DIAGONAL, MINUS_TWO_THIRDS_PI).toTransformMatrix());
+ }
+
+ @Test
+ public void testAxisAngleSequenceConversion_relative() {
+ for (AxisSequence axes : AxisSequence.values()) {
+ checkAxisAngleSequenceToQuaternionRoundtrip(AxisReferenceFrame.RELATIVE, axes);
+ checkQuaternionToAxisAngleSequenceRoundtrip(AxisReferenceFrame.RELATIVE, axes);
+ }
+ }
+
+ @Test
+ public void testAxisAngleSequenceConversion_absolute() {
+ for (AxisSequence axes : AxisSequence.values()) {
+ checkAxisAngleSequenceToQuaternionRoundtrip(AxisReferenceFrame.ABSOLUTE, axes);
+ checkQuaternionToAxisAngleSequenceRoundtrip(AxisReferenceFrame.ABSOLUTE, axes);
+ }
+ }
+
+ private void checkAxisAngleSequenceToQuaternionRoundtrip(AxisReferenceFrame frame, AxisSequence axes) {
+ double step = 0.3;
+ double angle2Start = axes.getType() == AxisSequenceType.EULER ? Geometry.ZERO_PI + 0.1 : Geometry.MINUS_HALF_PI + 0.1;
+ double angle2Stop = angle2Start + Geometry.PI;
+
+ for (double angle1 = Geometry.ZERO_PI; angle1 <= Geometry.TWO_PI; angle1 += step) {
+ for (double angle2 = angle2Start; angle2 < angle2Stop; angle2 += step) {
+ for (double angle3 = Geometry.ZERO_PI; angle3 <= Geometry.TWO_PI; angle3 += 0.3) {
+ // arrange
+ AxisAngleSequence angles = new AxisAngleSequence(frame, axes, angle1, angle2, angle3);
+
+ // act
+ QuaternionRotation q = QuaternionRotation.fromAxisAngleSequence(angles);
+ AxisAngleSequence result = q.toAxisAngleSequence(frame, axes);
+
+ // assert
+ Assert.assertEquals(frame, result.getReferenceFrame());
+ Assert.assertEquals(axes, result.getAxisSequence());
+
+ assertRadiansEquals(angle1, result.getAngle1());
+ assertRadiansEquals(angle2, result.getAngle2());
+ assertRadiansEquals(angle3, result.getAngle3());
+ }
+ }
+ }
+ }
+
+ private void checkQuaternionToAxisAngleSequenceRoundtrip(AxisReferenceFrame frame, AxisSequence axes) {
+ final double step = 0.1;
+
+ EuclideanTestUtils.permuteSkipZero(-1, 1, 0.5, (x, y, z) -> {
+ Vector3D axis = Vector3D.of(x, y, z);
+
+ for (double angle = -Geometry.TWO_PI; angle <= Geometry.TWO_PI; angle += step) {
+ // arrange
+ QuaternionRotation q = QuaternionRotation.fromAxisAngle(axis, angle);
+
+ // act
+ AxisAngleSequence seq = q.toAxisAngleSequence(frame, axes);
+ QuaternionRotation result = QuaternionRotation.fromAxisAngleSequence(seq);
+
+ // assert
+ checkQuaternion(result, q.getX(), q.getY(), q.getZ(), q.getW());
+ }
+ });
+ }
+
+ @Test
+ public void testAxisAngleSequenceConversion_relative_eulerSingularities() {
+ // arrange
+ double[] eulerSingularities = {
+ Geometry.ZERO_PI,
+ Geometry.PI
+ };
+
+ double angle1 = 0.1;
+ double angle2 = 0.3;
+
+ AxisReferenceFrame frame = AxisReferenceFrame.RELATIVE;
+
+ for (AxisSequence axes : getAxes(AxisSequenceType.EULER)) {
+ for (int i=0; i<eulerSingularities.length; ++i) {
+
+ double singularityAngle = eulerSingularities[i];
+
+ AxisAngleSequence inputSeq = new AxisAngleSequence(frame, axes, angle1, singularityAngle, angle2);
+ QuaternionRotation inputQuat = QuaternionRotation.fromAxisAngleSequence(inputSeq);
+
+ // act
+ AxisAngleSequence resultSeq = inputQuat.toAxisAngleSequence(frame, axes);
+ QuaternionRotation resultQuat = QuaternionRotation.fromAxisAngleSequence(resultSeq);
+
+ // assert
+ Assert.assertEquals(frame, resultSeq.getReferenceFrame());
+ Assert.assertEquals(axes, resultSeq.getAxisSequence());
+
+ assertRadiansEquals(singularityAngle, resultSeq.getAngle2());
+ assertRadiansEquals(0.0, resultSeq.getAngle3());
+
+ checkQuaternion(resultQuat, inputQuat.getX(), inputQuat.getY(), inputQuat.getZ(), inputQuat.getW());
+ }
+ }
+ }
+
+ @Test
+ public void testAxisAngleSequenceConversion_absolute_eulerSingularities() {
+ // arrange
+ double[] eulerSingularities = {
+ Geometry.ZERO_PI,
+ Geometry.PI
+ };
+
+ double angle1 = 0.1;
+ double angle2 = 0.3;
+
+ AxisReferenceFrame frame = AxisReferenceFrame.ABSOLUTE;
+
+ for (AxisSequence axes : getAxes(AxisSequenceType.EULER)) {
+ for (int i=0; i<eulerSingularities.length; ++i) {
+
+ double singularityAngle = eulerSingularities[i];
+
+ AxisAngleSequence inputSeq = new AxisAngleSequence(frame, axes, angle1, singularityAngle, angle2);
+ QuaternionRotation inputQuat = QuaternionRotation.fromAxisAngleSequence(inputSeq);
+
+ // act
+ AxisAngleSequence resultSeq = inputQuat.toAxisAngleSequence(frame, axes);
+ QuaternionRotation resultQuat = QuaternionRotation.fromAxisAngleSequence(resultSeq);
+
+ // assert
+ Assert.assertEquals(frame, resultSeq.getReferenceFrame());
+ Assert.assertEquals(axes, resultSeq.getAxisSequence());
+
+ assertRadiansEquals(0.0, resultSeq.getAngle1());
+ assertRadiansEquals(singularityAngle, resultSeq.getAngle2());
+
+ checkQuaternion(resultQuat, inputQuat.getX(), inputQuat.getY(), inputQuat.getZ(), inputQuat.getW());
+ }
+ }
+ }
+
+ @Test
+ public void testAxisAngleSequenceConversion_relative_taitBryanSingularities() {
+ // arrange
+ double[] taitBryanSingularities = {
+ Geometry.MINUS_HALF_PI,
+ Geometry.HALF_PI
+ };
+
+ double angle1 = 0.1;
+ double angle2 = 0.3;
+
+ AxisReferenceFrame frame = AxisReferenceFrame.RELATIVE;
+
+ for (AxisSequence axes : getAxes(AxisSequenceType.TAIT_BRYAN)) {
+ for (int i=0; i<taitBryanSingularities.length; ++i) {
+
+ double singularityAngle = taitBryanSingularities[i];
+
+ AxisAngleSequence inputSeq = new AxisAngleSequence(frame, axes, angle1, singularityAngle, angle2);
+ QuaternionRotation inputQuat = QuaternionRotation.fromAxisAngleSequence(inputSeq);
+
+ // act
+ AxisAngleSequence resultSeq = inputQuat.toAxisAngleSequence(frame, axes);
+ QuaternionRotation resultQuat = QuaternionRotation.fromAxisAngleSequence(resultSeq);
+
+ // assert
+ Assert.assertEquals(frame, resultSeq.getReferenceFrame());
+ Assert.assertEquals(axes, resultSeq.getAxisSequence());
+
+ assertRadiansEquals(singularityAngle, resultSeq.getAngle2());
+ assertRadiansEquals(0.0, resultSeq.getAngle3());
+
+ checkQuaternion(resultQuat, inputQuat.getX(), inputQuat.getY(), inputQuat.getZ(), inputQuat.getW());
+ }
+ }
+ }
+
+ @Test
+ public void testAxisAngleSequenceConversion_absolute_taitBryanSingularities() {
+ // arrange
+ double[] taitBryanSingularities = {
+ Geometry.MINUS_HALF_PI,
+ Geometry.HALF_PI
+ };
+
+ double angle1 = 0.1;
+ double angle2 = 0.3;
+
+ AxisReferenceFrame frame = AxisReferenceFrame.ABSOLUTE;
+
+ for (AxisSequence axes : getAxes(AxisSequenceType.TAIT_BRYAN)) {
+ for (int i=0; i<taitBryanSingularities.length; ++i) {
+
+ double singularityAngle = taitBryanSingularities[i];
+
+ AxisAngleSequence inputSeq = new AxisAngleSequence(frame, axes, angle1, singularityAngle, angle2);
+ QuaternionRotation inputQuat = QuaternionRotation.fromAxisAngleSequence(inputSeq);
+
+ // act
+ AxisAngleSequence resultSeq = inputQuat.toAxisAngleSequence(frame, axes);
+ QuaternionRotation resultQuat = QuaternionRotation.fromAxisAngleSequence(resultSeq);
+
+ // assert
+ Assert.assertEquals(frame, resultSeq.getReferenceFrame());
+ Assert.assertEquals(axes, resultSeq.getAxisSequence());
+
+ assertRadiansEquals(0.0, resultSeq.getAngle1());
+ assertRadiansEquals(singularityAngle, resultSeq.getAngle2());
+
+ checkQuaternion(resultQuat, inputQuat.getX(), inputQuat.getY(), inputQuat.getZ(), inputQuat.getW());
+ }
+ }
+ }
+
+ private List<AxisSequence> getAxes(final AxisSequenceType type) {
+ return Arrays.asList(AxisSequence.values()).stream()
+ .filter(a -> type.equals(a.getType()))
+ .collect(Collectors.toList());
+ }
+
+ @Test
+ public void testToAxisAngleSequence_invalidArgs() {
+ // arrange
+ QuaternionRotation q = QuaternionRotation.identity();
+
+ // act/assert
+ GeometryTestUtils.assertThrows(() -> q.toAxisAngleSequence(null, AxisSequence.XYZ), IllegalArgumentException.class);
+ GeometryTestUtils.assertThrows(() -> q.toAxisAngleSequence(AxisReferenceFrame.ABSOLUTE, null), IllegalArgumentException.class);
+ }
+
+ @Test
+ public void testToRelativeAxisAngleSequence() {
+ // arrange
+ QuaternionRotation q = QuaternionRotation.fromAxisAngle(PLUS_DIAGONAL, TWO_THIRDS_PI);
+
+ // act
+ AxisAngleSequence seq = q.toRelativeAxisAngleSequence(AxisSequence.YZX);
+
+ // assert
+ Assert.assertEquals(AxisReferenceFrame.RELATIVE, seq.getReferenceFrame());
+ Assert.assertEquals(AxisSequence.YZX, seq.getAxisSequence());
+ Assert.assertEquals(Geometry.HALF_PI, seq.getAngle1(), EPS);
+ Assert.assertEquals(Geometry.HALF_PI, seq.getAngle2(), EPS);
+ Assert.assertEquals(0, seq.getAngle3(), EPS);
+ }
+
+ @Test
+ public void testToAbsoluteAxisAngleSequence() {
+ // arrange
+ QuaternionRotation q = QuaternionRotation.fromAxisAngle(PLUS_DIAGONAL, TWO_THIRDS_PI);
+
+ // act
+ AxisAngleSequence seq = q.toAbsoluteAxisAngleSequence(AxisSequence.YZX);
+
+ // assert
+ Assert.assertEquals(AxisReferenceFrame.ABSOLUTE, seq.getReferenceFrame());
+ Assert.assertEquals(AxisSequence.YZX, seq.getAxisSequence());
+ Assert.assertEquals(Geometry.HALF_PI, seq.getAngle1(), EPS);
+ Assert.assertEquals(0, seq.getAngle2(), EPS);
+ Assert.assertEquals(Geometry.HALF_PI, seq.getAngle3(), EPS);
+ }
+
+ @Test
+ public void testHashCode() {
+ // arrange
+ double delta = 100 * Precision.EPSILON;
+ QuaternionRotation q1 = QuaternionRotation.of(1, 2, 3, 4);
+ QuaternionRotation q2 = QuaternionRotation.of(1, 2, 3, 4);
+
+ // act/assert
+ Assert.assertEquals(q1.hashCode(), q2.hashCode());
+
+ Assert.assertNotEquals(q1.hashCode(), QuaternionRotation.of(1 + delta, 2, 3, 4).hashCode());
+ Assert.assertNotEquals(q1.hashCode(), QuaternionRotation.of(1, 2 + delta, 3, 4).hashCode());
+ Assert.assertNotEquals(q1.hashCode(), QuaternionRotation.of(1, 2, 3 + delta, 4).hashCode());
+ Assert.assertNotEquals(q1.hashCode(), QuaternionRotation.of(1, 2, 3, 4 + delta).hashCode());
+ }
+
+ @Test
+ public void testEquals() {
+ // arrange
+ double delta = 100 * Precision.EPSILON;
+ QuaternionRotation q1 = QuaternionRotation.of(1, 2, 3, 4);
+ QuaternionRotation q2 = QuaternionRotation.of(1, 2, 3, 4);
+
+ // act/assert
+ Assert.assertFalse(q1.equals(null));
+ Assert.assertFalse(q1.equals(new Object()));
+
+ Assert.assertTrue(q1.equals(q1));
+ Assert.assertTrue(q1.equals(q2));
+
+ Assert.assertFalse(q1.equals(QuaternionRotation.of(-1, -2, -3, 4)));
+ Assert.assertFalse(q1.equals(QuaternionRotation.of(1, 2, 3, -4)));
+
+ Assert.assertFalse(q1.equals(QuaternionRotation.of(1 + delta, 2, 3, 4)));
+ Assert.assertFalse(q1.equals(QuaternionRotation.of(1, 2 + delta, 3, 4)));
+ Assert.assertFalse(q1.equals(QuaternionRotation.of(1, 2, 3 + delta, 4)));
+ Assert.assertFalse(q1.equals(QuaternionRotation.of(1, 2, 3, 4 + delta)));
+ }
+
+ @Test
+ public void testToString() {
+ // arrange
+ QuaternionRotation q = QuaternionRotation.of(1, 2, 3, 4);
+ Pattern pattern = Pattern.compile("\\(0\\.1\\d+, 0\\.3\\d+, 0\\.5\\d+, 0\\.7\\d+\\)");
+
+ // act
+ String str = q.toString();
+
+ // assert
+ Assert.assertTrue("Expected string " + str + " to match regex " + pattern,
+ pattern.matcher(str).matches());
+ }
+
+ @Test
+ public void testCreateVectorRotation_simple() {
+ // arrange
+ Vector3D u1 = Vector3D.PLUS_X;
+ Vector3D u2 = Vector3D.PLUS_Y;
+
+ // act
+ QuaternionRotation q = QuaternionRotation.createVectorRotation(u1, u2);
+
+ // assert
+ double val = Math.sqrt(2) * 0.5;
+
+ checkQuaternion(q, 0, 0, val, val);
+
+ EuclideanTestUtils.assertCoordinatesEqual(Vector3D.PLUS_Z, q.getAxis(), EPS);
+ Assert.assertEquals(Geometry.HALF_PI, q.getAngle(), EPS);
+
+ EuclideanTestUtils.assertCoordinatesEqual(u2, q.apply(u1), EPS);
+ EuclideanTestUtils.assertCoordinatesEqual(u1, q.getInverse().apply(u2), EPS);
+ }
+
+ @Test
+ public void testCreateVectorRotation_identity() {
+ // arrange
+ Vector3D u1 = Vector3D.of(0, 2, 0);
+ Vector3D u2 = u1;
+
+ // act
+ QuaternionRotation q = QuaternionRotation.createVectorRotation(u1, u2);
+
+ // assert
+ checkQuaternion(q, 0, 0, 0, 1);
+
+ EuclideanTestUtils.assertCoordinatesEqual(Vector3D.PLUS_X, q.getAxis(), EPS);
+ Assert.assertEquals(Geometry.ZERO_PI, q.getAngle(), EPS);
+
+ EuclideanTestUtils.assertCoordinatesEqual(Vector3D.of(0, 2, 0), q.apply(u1), EPS);
+ EuclideanTestUtils.assertCoordinatesEqual(Vector3D.of(0, 2, 0), q.getInverse().apply(u2), EPS);
+ }
+
+ @Test
+ public void testCreateVectorRotation_parallel() {
+ // arrange
+ Vector3D u1 = Vector3D.of(0, 2, 0);
+ Vector3D u2 = Vector3D.of(0, 3, 0);
+
+ // act
+ QuaternionRotation q = QuaternionRotation.createVectorRotation(u1, u2);
+
+ // assert
+ checkQuaternion(q, 0, 0, 0, 1);
+
+ EuclideanTestUtils.assertCoordinatesEqual(Vector3D.PLUS_X, q.getAxis(), EPS);
+ Assert.assertEquals(Geometry.ZERO_PI, q.getAngle(), EPS);
+
+ EuclideanTestUtils.assertCoordinatesEqual(Vector3D.of(0, 2, 0), q.apply(u1), EPS);
+ EuclideanTestUtils.assertCoordinatesEqual(Vector3D.of(0, 3, 0), q.getInverse().apply(u2), EPS);
+ }
+
+ @Test
+ public void testCreateVectorRotation_antiparallel() {
+ // arrange
+ Vector3D u1 = Vector3D.of(0, 2, 0);
+ Vector3D u2 = Vector3D.of(0, -3, 0);
+
+ // act
+ QuaternionRotation q = QuaternionRotation.createVectorRotation(u1, u2);
+
+ // assert
+ Vector3D axis = q.getAxis();
+ Assert.assertEquals(0.0, axis.dotProduct(u1), EPS);
+ Assert.assertEquals(0.0, axis.dotProduct(u2), EPS);
+ Assert.assertEquals(Geometry.PI, q.getAngle(), EPS);
+
+ EuclideanTestUtils.assertCoordinatesEqual(Vector3D.of(0, -2, 0), q.apply(u1), EPS);
+ EuclideanTestUtils.assertCoordinatesEqual(Vector3D.of(0, 3, 0), q.getInverse().apply(u2), EPS);
+ }
+
+ @Test
+ public void testCreateVectorRotation_permute() {
+ EuclideanTestUtils.permuteSkipZero(-5, 5, 0.1, (x, y, z) -> {
+ // arrange
+ Vector3D u1 = Vector3D.of(x, y, z);
+ Vector3D u2 = PLUS_DIAGONAL;
+
+ // act
+ QuaternionRotation q = QuaternionRotation.createVectorRotation(u1, u2);
+
+ // assert
+ Assert.assertEquals(0.0, q.apply(u1).angle(u2), EPS);
+ Assert.assertEquals(0.0, q.getInverse().apply(u2).angle(u1), EPS);
+
+ double angle = q.getAngle();
+ Assert.assertTrue(angle >= Geometry.ZERO_PI);
+ Assert.assertTrue(angle <= Geometry.PI);
+ });
+ }
+
+ @Test
+ public void testCreateVectorRotation_invalidArgs() {
+ // act/assert
+ GeometryTestUtils.assertThrows(() -> QuaternionRotation.createVectorRotation(Vector3D.ZERO, Vector3D.PLUS_X),
+ IllegalNormException.class);
+ GeometryTestUtils.assertThrows(() -> QuaternionRotation.createVectorRotation(Vector3D.PLUS_X, Vector3D.ZERO),
+ IllegalNormException.class);
+
+ GeometryTestUtils.assertThrows(() -> QuaternionRotation.createVectorRotation(Vector3D.NaN, Vector3D.PLUS_X),
+ IllegalNormException.class);
+ GeometryTestUtils.assertThrows(() -> QuaternionRotation.createVectorRotation(Vector3D.PLUS_X, Vector3D.POSITIVE_INFINITY),
+ IllegalNormException.class);
+ GeometryTestUtils.assertThrows(() -> QuaternionRotation.createVectorRotation(Vector3D.PLUS_X, Vector3D.NEGATIVE_INFINITY),
+ IllegalNormException.class);
+ }
+
+ @Test
+ public void testCreateBasisRotation_simple() {
+ // arrange
+ Vector3D u1 = Vector3D.PLUS_X;
+ Vector3D u2 = Vector3D.PLUS_Y;
+
+ Vector3D v1 = Vector3D.PLUS_Y;
+ Vector3D v2 = Vector3D.MINUS_X;
+
+ // act
+ QuaternionRotation q = QuaternionRotation.createBasisRotation(u1, u2, v1, v2);
+
+ // assert
+ QuaternionRotation qInv = q.getInverse();
+
+ EuclideanTestUtils.assertCoordinatesEqual(v1, q.apply(u1), EPS);
+ EuclideanTestUtils.assertCoordinatesEqual(v2, q.apply(u2), EPS);
+
+ EuclideanTestUtils.assertCoordinatesEqual(u1, qInv.apply(v1), EPS);
+ EuclideanTestUtils.assertCoordinatesEqual(u2, qInv.apply(v2), EPS);
+
+ assertRotationEquals(StandardRotations.PLUS_Z_HALF_PI, q);
+ }
+
+ @Test
+ public void testCreateBasisRotation_diagonalAxis() {
+ // arrange
+ Vector3D u1 = Vector3D.PLUS_X;
+ Vector3D u2 = Vector3D.PLUS_Y;
+
+ Vector3D v1 = Vector3D.PLUS_Y;
+ Vector3D v2 = Vector3D.PLUS_Z;
+
+ // act
+ QuaternionRotation q = QuaternionRotation.createBasisRotation(u1, u2, v1, v2);
+
+ // assert
+ QuaternionRotation qInv = q.getInverse();
+
+ EuclideanTestUtils.assertCoordinatesEqual(v1, q.apply(u1), EPS);
+ EuclideanTestUtils.assertCoordinatesEqual(v2, q.apply(u2), EPS);
+
+ EuclideanTestUtils.assertCoordinatesEqual(u1, qInv.apply(v1), EPS);
+ EuclideanTestUtils.assertCoordinatesEqual(u2, qInv.apply(v2), EPS);
+
+ assertRotationEquals(StandardRotations.PLUS_DIAGONAL_TWO_THIRDS_PI, q);
+ assertRotationEquals(StandardRotations.MINUS_DIAGONAL_TWO_THIRDS_PI, q.getInverse());
+ }
+
+ @Test
+ public void testCreateBasisRotation_identity() {
+ // arrange
+ Vector3D u1 = Vector3D.PLUS_X;
+ Vector3D u2 = Vector3D.PLUS_Y;
+
+ Vector3D v1 = u1;
+ Vector3D v2 = u2;
+
+ // act
+ QuaternionRotation q = QuaternionRotation.createBasisRotation(u1, u2, v1, v2);
+
+ // assert
+ QuaternionRotation qInv = q.getInverse();
+
+ EuclideanTestUtils.assertCoordinatesEqual(v1, q.apply(u1), EPS);
+ EuclideanTestUtils.assertCoordinatesEqual(v2, q.apply(u2), EPS);
+
+ EuclideanTestUtils.assertCoordinatesEqual(u1, qInv.apply(v1), EPS);
+ EuclideanTestUtils.assertCoordinatesEqual(u2, qInv.apply(v2), EPS);
+
+ assertRotationEquals(StandardRotations.IDENTITY, q);
+ }
+
+ @Test
+ public void testCreateBasisRotation_equivalentBases() {
+ // arrange
+ Vector3D u1 = Vector3D.of(2, 0, 0);
+ Vector3D u2 = Vector3D.of(0, 3, 0);
+
+ Vector3D v1 = Vector3D.of(4, 0, 0);
+ Vector3D v2 = Vector3D.of(0, 5, 0);
+
+ // act
+ QuaternionRotation q = QuaternionRotation.createBasisRotation(u1, u2, v1, v2);
+
+ // assert
+ QuaternionRotation qInv = q.getInverse();
+
+ EuclideanTestUtils.assertCoordinatesEqual(u1, q.apply(u1), EPS);
+ EuclideanTestUtils.assertCoordinatesEqual(u2, q.apply(u2), EPS);
+
+ EuclideanTestUtils.assertCoordinatesEqual(v1, qInv.apply(v1), EPS);
+ EuclideanTestUtils.assertCoordinatesEqual(v2, qInv.apply(v2), EPS);
+
+ assertRotationEquals(StandardRotations.IDENTITY, q);
+ }
+
+ @Test
+ public void testCreateBasisRotation_nonOrthogonalVectors() {
+ // arrange
+ Vector3D u1 = Vector3D.of(2, 0, 0);
+ Vector3D u2 = Vector3D.of(1, 0.5, 0);
+
+ Vector3D v1 = Vector3D.of(0, 1.5, 0);
+ Vector3D v2 = Vector3D.of(-1, 1.5, 0);
+
+ // act
+ QuaternionRotation q = QuaternionRotation.createBasisRotation(u1, u2, v1, v2);
+
+ // assert
+ QuaternionRotation qInv = q.getInverse();
+
+ EuclideanTestUtils.assertCoordinatesEqual(Vector3D.of(0, 2, 0), q.apply(u1), EPS);
+ EuclideanTestUtils.assertCoordinatesEqual(Vector3D.of(-0.5, 1, 0), q.apply(u2), EPS);
+
+ EuclideanTestUtils.assertCoordinatesEqual(Vector3D.of(1.5, 0, 0), qInv.apply(v1), EPS);
+ EuclideanTestUtils.assertCoordinatesEqual(Vector3D.of(1.5, 1, 0), qInv.apply(v2), EPS);
+
+ assertRotationEquals(StandardRotations.PLUS_Z_HALF_PI, q);
+ }
+
+ @Test
+ public void testCreateBasisRotation_permute() {
+ // arrange
+ Vector3D u1 = Vector3D.of(1, 2, 3);
+ Vector3D u2 = Vector3D.of(0, 4, 0);
+
+ Vector3D u1Dir = u1.normalize();
+ Vector3D u2Dir = u1Dir.orthogonal(u2);
+
+ EuclideanTestUtils.permuteSkipZero(-5, 5, 0.2, (x, y, z) -> {
+ Vector3D v1 = Vector3D.of(x, y, z);
+ Vector3D v2 = v1.orthogonal();
+
+ Vector3D v1Dir = v1.normalize();
+ Vector3D v2Dir = v2.normalize();
+
+ // act
+ QuaternionRotation q = QuaternionRotation.createBasisRotation(u1, u2, v1, v2);
+ QuaternionRotation qInv = q.getInverse();
+
+ // assert
+ EuclideanTestUtils.assertCoordinatesEqual(v1Dir, q.apply(u1Dir), EPS);
+ EuclideanTestUtils.assertCoordinatesEqual(v2Dir, q.apply(u2Dir), EPS);
+
+ EuclideanTestUtils.assertCoordinatesEqual(u1Dir, qInv.apply(v1Dir), EPS);
+ EuclideanTestUtils.assertCoordinatesEqual(u2Dir, qInv.apply(v2Dir), EPS);
+
+ double angle = q.getAngle();
+ Assert.assertTrue(angle >= Geometry.ZERO_PI);
+ Assert.assertTrue(angle <= Geometry.PI);
+
+ Vector3D transformedX = q.apply(Vector3D.PLUS_X);
+ Vector3D transformedY = q.apply(Vector3D.PLUS_Y);
+ Vector3D transformedZ = q.apply(Vector3D.PLUS_Z);
+
+ Assert.assertEquals(1.0, transformedX.getNorm(), EPS);
+ Assert.assertEquals(1.0, transformedY.getNorm(), EPS);
+ Assert.assertEquals(1.0, transformedZ.getNorm(), EPS);
... 1561 lines suppressed ...