darkma773r closed pull request #23: NUMBERS-86: Adding Slerp Unit Tests
URL: https://github.com/apache/commons-numbers/pull/23
This is a PR merged from a forked repository.
As GitHub hides the original diff on merge, it is displayed below for
the sake of provenance:
As this is a foreign pull request (from a fork), the diff is supplied
below (as it won't show otherwise due to GitHub magic):
diff --git
a/commons-numbers-quaternion/src/main/java/org/apache/commons/numbers/quaternion/Slerp.java
b/commons-numbers-quaternion/src/main/java/org/apache/commons/numbers/quaternion/Slerp.java
index b6f4b3b4..c2004329 100644
---
a/commons-numbers-quaternion/src/main/java/org/apache/commons/numbers/quaternion/Slerp.java
+++
b/commons-numbers-quaternion/src/main/java/org/apache/commons/numbers/quaternion/Slerp.java
@@ -23,7 +23,9 @@
*
* The <em>Slerp</em> algorithm is 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 original purpose of this algorithm was to animate 3D rotations. All
output
+ * quaternions are in positive polar form, meaning a unit quaternion with a
positive
+ * scalar component.
*/
public class Slerp implements DoubleFunction<Quaternion> {
/**
@@ -70,15 +72,26 @@ public Slerp(Quaternion start,
* Performs the interpolation.
* The rotation returned by this method is controlled by the interpolation
parameter, {@code t}.
* All other values are interpolated (or extrapolated if {@code t} is
outside of the
- * {@code [0, 1]} range).
+ * {@code [0, 1]} range). The returned quaternion is in positive polar
form, meaning that it
+ * is a unit quaternion with a positive scalar component.
*
* @param t Interpolation control parameter.
* When {@code t = 0}, a rotation equal to the start instance is returned.
* When {@code t = 1}, a rotation equal to the end instance is returned.
- * @return an interpolated quaternion.
+ * @return an interpolated quaternion in positive polar form.
*/
@Override
public Quaternion apply(double t) {
+ return computeQuaternion(t).positivePolarForm();
+ }
+
+ /**
+ * Compute the quaternion for the given interpolation control parameter.
The return
+ * value need not be in positive polar form.
+ * @param t interpolation control parameter
+ * @return an interpolated quaternion
+ */
+ private Quaternion computeQuaternion(double t) {
// Handle no-op cases.
if (t == 0) {
return start;
@@ -90,7 +103,7 @@ public Quaternion apply(double t) {
}
/**
- * Linear interpolation, used when the quaternions are too closely
aligned.
+ * Linear interpolation, used when the quaternions are too closely aligned.
*/
private class Linear implements DoubleFunction<Quaternion> {
/** Default constructor. */
@@ -103,12 +116,12 @@ public Quaternion apply(double t) {
return Quaternion.of(f * start.getW() + t * end.getW(),
f * start.getX() + t * end.getX(),
f * start.getY() + t * end.getY(),
- f * start.getZ() + t *
end.getZ()).positivePolarForm();
+ f * start.getZ() + t * end.getZ());
}
}
/**
- * Spherical interpolation, used whe the quaternions are too closely
aligned.
+ * Spherical interpolation, used when the quaternions are too closely
aligned.
* When we may end up dividing by zero (cf. 1/sin(theta) term below).
* {@link Linear} interpolation must be used.
*/
@@ -135,7 +148,7 @@ public Quaternion apply(double t) {
return Quaternion.of(f1 * start.getW() + f2 * end.getW(),
f1 * start.getX() + f2 * end.getX(),
f1 * start.getY() + f2 * end.getY(),
- f1 * start.getZ() + f2 *
end.getZ()).positivePolarForm();
+ f1 * start.getZ() + f2 * end.getZ());
}
}
}
diff --git
a/commons-numbers-quaternion/src/test/java/org/apache/commons/numbers/quaternion/SlerpTest.java
b/commons-numbers-quaternion/src/test/java/org/apache/commons/numbers/quaternion/SlerpTest.java
index ac61633e..c966fe3e 100644
---
a/commons-numbers-quaternion/src/test/java/org/apache/commons/numbers/quaternion/SlerpTest.java
+++
b/commons-numbers-quaternion/src/test/java/org/apache/commons/numbers/quaternion/SlerpTest.java
@@ -16,9 +16,374 @@
*/
package org.apache.commons.numbers.quaternion;
-import org.junit.Test;
+import org.apache.commons.numbers.core.Precision;
import org.junit.Assert;
+import org.junit.Test;
public class SlerpTest {
- // TODO
+
+ private static final double EPS = 1e-7;
+
+ private static final double SQRT_2 = Math.sqrt(2.0);
+ private static final double INV_SQRT_2 = 1.0 / SQRT_2;
+
+ @Test
+ public void testSlerp_sphericalAlgorithm() {
+ // arrange
+ Quaternion q1 = createZRotation(0.75 * Math.PI);
+ Quaternion q2 = createZRotation(-0.75 * Math.PI);
+
+ Slerp slerp = new Slerp(q1, q2);
+
+ // act/assert
+ // the algorithm should follow the path around the pi coordinate of
the circle rather than
+ // the one through the zero coordinate
+ assertQuaternion(q1.positivePolarForm(), slerp.apply(0));
+ assertQuaternion(createZRotation(0.875 * Math.PI), slerp.apply(0.25));
+ assertQuaternion(createZRotation(Math.PI), slerp.apply(0.5));
+ assertQuaternion(createZRotation(-0.875 * Math.PI), slerp.apply(0.75));
+ assertQuaternion(q2.positivePolarForm(), slerp.apply(1));
+ }
+
+ @Test
+ public void
testSlerp_sphericalAlgorithm_allOutputsAreInPositivePolarForm() {
+ // arrange
+ Quaternion q1 = createZRotation(0.75 * Math.PI);
+ Quaternion q2 = createZRotation(-0.75 * Math.PI);
+
+ Slerp slerp = new Slerp(q1, q2);
+
+ final int numSteps = 200;
+ final double delta = 1d / numSteps;
+ for (int step = 0; step <= numSteps; step++) {
+ final double t = -10 + step * delta;
+
+ // act
+ Quaternion result = slerp.apply(t);
+
+ // assert
+ Assert.assertEquals(1.0, result.norm(), EPS);
+ Assert.assertTrue(result.getW() >= 0.0);
+ }
+ }
+
+ @Test
+ public void testSlerp_nonNormalizedInputs() {
+ // arrange
+ Quaternion q1 = createZRotation(0).multiply(10.0);
+ Quaternion q2 = createZRotation(Math.PI).multiply(0.2);
+
+ Slerp slerp = new Slerp(q1, q2);
+
+ // act/assert
+ assertQuaternion(q1.positivePolarForm(), slerp.apply(0));
+ assertQuaternion(createZRotation(0.25 * Math.PI), slerp.apply(0.25));
+ assertQuaternion(createZRotation(0.5 * Math.PI), slerp.apply(0.5));
+ assertQuaternion(createZRotation(0.75 * Math.PI), slerp.apply(0.75));
+ assertQuaternion(q2.positivePolarForm(), slerp.apply(1));
+ }
+
+ @Test
+ public void testSlerp_linearAlgorithm() {
+ // arrange
+ Quaternion q1 = createZRotation(0.75 * Math.PI);
+ Quaternion q2 = createZRotation(0.76 * Math.PI);
+
+ Slerp slerp = new Slerp(q1, q2);
+
+ // act/assert
+ assertQuaternion(q1.positivePolarForm(), slerp.apply(0));
+ assertQuaternion(createZRotation(0.7525 * Math.PI), slerp.apply(0.25));
+ assertQuaternion(createZRotation(0.755 * Math.PI), slerp.apply(0.5));
+ assertQuaternion(createZRotation(0.7575 * Math.PI), slerp.apply(0.75));
+ assertQuaternion(q2.positivePolarForm(), slerp.apply(1));
+ }
+
+ @Test
+ public void testSlerp_linearAlgorithm_allOutputsAreInPositivePolarForm() {
+ // arrange
+ Quaternion q1 = createZRotation(0.75 * Math.PI);
+ Quaternion q2 = createZRotation(0.76 * Math.PI);
+
+ Slerp slerp = new Slerp(q1, q2);
+
+ final int numSteps = 200;
+ final double delta = 1d / numSteps;
+ for (int step = 0; step <= numSteps; step++) {
+ final double t = -10 + step * delta;
+
+ // act
+ Quaternion result = slerp.apply(t);
+
+ // assert
+ Assert.assertEquals(1.0, result.norm(), EPS);
+ Assert.assertTrue(result.getW() >= 0.0);
+ }
+ }
+
+ @Test
+ public void testSlerp_identicalInputs() {
+ // arrange
+ Quaternion q1 = createZRotation(0);
+ Quaternion q2 = createZRotation(0);
+
+ Slerp slerp = new Slerp(q1, q2);
+
+ // act/assert
+ Quaternion expected = q1.positivePolarForm();
+
+ assertQuaternion(expected, slerp.apply(0));
+ assertQuaternion(expected, slerp.apply(0.5));
+ assertQuaternion(expected, slerp.apply(1));
+ }
+
+ @Test
+ public void testSlerp_inputQuaternionsHaveMinusOneDotProduct() {
+ // arrange
+ Quaternion q1 = createZRotation(0.5 * Math.PI);
+ Quaternion q2 = createZRotation(1.5 * Math.PI).conjugate(); // 3pi/2
around -z
+
+ Slerp slerp = new Slerp(q1, q2);
+
+ // act/assert
+ Assert.assertEquals(-1.0, q1.dotProduct(q2), EPS);
+
+ Quaternion expected = q1.positivePolarForm();
+
+ assertQuaternion(expected, slerp.apply(0));
+ assertQuaternion(expected, slerp.apply(0.5));
+ assertQuaternion(expected, slerp.apply(1));
+ }
+
+ @Test
+ public void testSlerp_tOutsideOfZeroToOne() {
+ // arrange
+ Quaternion q1 = createZRotation(0);
+ Quaternion q2 = createZRotation(0.25 * Math.PI);
+
+ Slerp slerp = new Slerp(q1, q2);
+
+ // act/assert
+ assertQuaternion(createZRotation(-0.5 * Math.PI).positivePolarForm(),
slerp.apply(-2));
+ assertQuaternion(createZRotation(-0.25 * Math.PI).positivePolarForm(),
slerp.apply(-1));
+
+ assertQuaternion(createZRotation(0).positivePolarForm(),
slerp.apply(0));
+
+ assertQuaternion(createZRotation(0.25 * Math.PI), slerp.apply(1));
+ assertQuaternion(createZRotation(0.5 * Math.PI), slerp.apply(2));
+ }
+
+ @Test
+ public void testVectorTransform_simple() {
+ // arrange
+ Quaternion q0 = Quaternion.of(1, 0, 0, 0); // rotation of zero
+ Quaternion q1 = Quaternion.of(0, 0, 0, 1); // pi rotation around +z
+
+ Slerp slerp = new Slerp(q0, q1);
+
+ double[] vec = { 2, 0, 1 };
+
+ // act/assert
+ Assert.assertArrayEquals(new double[] { 2, 0, 1 },
+ transformVector(slerp.apply(0), vec), EPS);
+
+ Assert.assertArrayEquals(new double[] { SQRT_2, SQRT_2, 1 },
+ transformVector(slerp.apply(0.25), vec), EPS);
+
+ Assert.assertArrayEquals(new double[] { 0, 2, 1 },
+ transformVector(slerp.apply(0.5), vec), EPS);
+
+ Assert.assertArrayEquals(new double[] { -SQRT_2, SQRT_2, 1 },
+ transformVector(slerp.apply(0.75), vec), EPS);
+
+ Assert.assertArrayEquals(new double[] { -2, 0, 1 },
+ transformVector(slerp.apply(1), vec), EPS);
+ }
+
+ @Test
+ public void testVectorTransform_multipleCombinations() {
+ // arrange
+ Quaternion[] quaternions = {
+ // +x axis
+ Quaternion.of(1, 0, 0, 0), // 0 pi
+ Quaternion.of(INV_SQRT_2, INV_SQRT_2, 0, 0), // pi/2
+ Quaternion.of(0, 1, 0, 0), // pi
+
+ // -x axis
+ Quaternion.of(1, 0, 0, 0), // 0 pi
+ Quaternion.of(INV_SQRT_2, -INV_SQRT_2, 0, 0), // pi/2
+ Quaternion.of(0, -1, 0, 0), // pi
+
+ // +y axis
+ Quaternion.of(1, 0, 0, 0), // 0 pi
+ Quaternion.of(INV_SQRT_2, 0, INV_SQRT_2, 0), // pi/2
+ Quaternion.of(0, 0, 1, 0), // pi
+
+ // -y axis
+ Quaternion.of(1, 0, 0, 0), // 0 pi
+ Quaternion.of(INV_SQRT_2, 0, -INV_SQRT_2, 0), // pi/2
+ Quaternion.of(0, 0, -1, 0), // pi
+
+ // +z axis
+ Quaternion.of(1, 0, 0, 0), // 0 pi
+ Quaternion.of(INV_SQRT_2, 0, 0, INV_SQRT_2), // pi/2
+ Quaternion.of(0, 0, 0, 1), // pi
+
+ // -z axis
+ Quaternion.of(1, 0, 0, 0), // 0 pi
+ Quaternion.of(INV_SQRT_2, 0, 0, -INV_SQRT_2), // pi/2
+ Quaternion.of(0, 0, 0, -1) // pi
+ };
+
+ // act/assert
+ // test each quaternion against all of the others (including itself)
+ for (int i=0; i<quaternions.length; ++i) {
+ for (int j=0; j<quaternions.length; ++j) {
+ checkSlerpCombination(quaternions[i], quaternions[j]);
+ }
+ }
+ }
+
+ private void checkSlerpCombination(Quaternion start, Quaternion end) {
+ Slerp slerp = new Slerp(start, end);
+
+ double[] vec = { 1, 2, 3 };
+ double vecNorm = norm(vec);
+
+ double[] startVec = transformVector(start, vec);
+ double[] endVec = transformVector(end, vec);
+
+ // check start and end values
+ Assert.assertArrayEquals(startVec, transformVector(slerp.apply(0),
vec), EPS);
+ Assert.assertArrayEquals(endVec, transformVector(slerp.apply(1), vec),
EPS);
+
+ // check intermediate values
+ double prevAngle = -1;
+ final int numSteps = 100;
+ final double delta = 1.0 / numSteps;
+ for (int step = 0; step <= numSteps; ++step) {
+ final double t= step * delta;
+ Quaternion result = slerp.apply(t);
+
+ double[] slerpVec = transformVector(result, vec);
+
+ // the transformation should not effect the vector magnitude
+ Assert.assertEquals(vecNorm, norm(slerpVec), EPS);
+
+ // make sure that we're steadily progressing to the end angle
+ double angle = angle(slerpVec, startVec);
+ Assert.assertTrue("Expected slerp angle to continuously increase;
previous angle was " +
+ prevAngle + " and new angle is " + angle,
+ Precision.compareTo(angle, prevAngle, EPS) >= 0);
+ }
+ }
+
+ @Test
+ public void testVectorTransform_tOutsideOfZeroToOne_() {
+ // arrange
+ double angle1 = Math.PI * 0.25;
+ double angle2 = Math.PI * 0.75;
+
+ double halfAngle1 = 0.5 * angle1;
+ double halfAngle2 = 0.5 * angle2;
+
+ Quaternion q1 = Quaternion.of(Math.cos(halfAngle1), 0, 0,
Math.sin(halfAngle1)); // pi/4 around +z
+ Quaternion q2 = Quaternion.of(Math.cos(halfAngle2), 0, 0,
Math.sin(halfAngle2)); // 3pi/4 around +z
+
+ double[] vec = new double[] { 1, 0, 0 };
+
+ // act/assert
+ Slerp slerp12 = new Slerp(q1, q2);
+ Assert.assertArrayEquals(new double[] { 1, 0, 0 },
transformVector(slerp12.apply(-4.5), vec), EPS);
+ Assert.assertArrayEquals(new double[] { 1, 0, 0 },
transformVector(slerp12.apply(-0.5), vec), EPS);
+ Assert.assertArrayEquals(new double[] { -1, 0, 0 },
transformVector(slerp12.apply(1.5), vec), EPS);
+ Assert.assertArrayEquals(new double[] { -1, 0, 0 },
transformVector(slerp12.apply(5.5), vec), EPS);
+
+ Slerp slerp21 = new Slerp(q2, q1);
+ Assert.assertArrayEquals(new double[] { -1, 0, 0 },
transformVector(slerp21.apply(-4.5), vec), EPS);
+ Assert.assertArrayEquals(new double[] { -1, 0, 0 },
transformVector(slerp21.apply(-0.5), vec), EPS);
+ Assert.assertArrayEquals(new double[] { 1, 0, 0 },
transformVector(slerp21.apply(1.5), vec), EPS);
+ Assert.assertArrayEquals(new double[] { 1, 0, 0 },
transformVector(slerp21.apply(5.5), vec), EPS);
+ }
+
+ /**
+ * Create a quaterion representing a rotation around the +z axis.
+ * @param theta
+ * @return
+ */
+ private static Quaternion createZRotation(final double theta) {
+ double halfAngle = theta * 0.5;
+
+ return Quaternion.of(Math.cos(halfAngle), 0, 0, Math.sin(halfAngle));
+ }
+
+ /**
+ * Compute the norm of a vector.
+ * @param vec
+ * @return
+ */
+ private static double norm(double[] vec) {
+ double sum = 0.0;
+ for (int i=0; i<vec.length; ++i) {
+ sum += vec[i] * vec[i];
+ }
+ return Math.sqrt(sum);
+ }
+
+ /**
+ * Compute the angle between two vectors.
+ * @param a
+ * @param b
+ * @return
+ */
+ private static double angle(double[] a, double[] b) {
+ double cos = dot(a, b) / (norm(a) * norm(b));
+ return Math.acos(cos);
+ }
+
+ /**
+ * Compute the dot product of two vectors. The arrays are assumed to
+ * have the same length.
+ * @param a
+ * @param b
+ * @return
+ */
+ private static double dot(double[] a, double[] b) {
+ double result = 0.0;
+ for (int i=0; i<a.length; ++i) {
+ result += a[i] * b[i];
+ }
+ return result;
+ }
+
+ /**
+ * Tranform the vector by assigning its components to the vectorial part
of a quaternion
+ * and then multiplying it on the right by the quaternion and the left by
the quaternion's
+ * conjugate (inverse).
+ * @param q the quaternion instance
+ * @param vec the 3D vector to transform
+ * @return the transformed 3D vector
+ */
+ private static double[] transformVector(Quaternion q, double[] vec) {
+ Quaternion qVec = Quaternion.of(0, vec[0], vec[1], vec[2]);
+ Quaternion qConj = q.conjugate();
+
+ Quaternion result = q.multiply(qVec).multiply(qConj);
+
+ return new double[] { result.getX(), result.getY(), result.getZ() };
+ }
+
+ /**
+ * Assert that the given quaternions are equal.
+ * @param expected
+ * @param actual
+ */
+ private static void assertQuaternion(Quaternion expected, Quaternion
actual) {
+ String msg = "Expected quaternion to equal " + expected + " but was "
+ actual;
+
+ Assert.assertEquals(msg, expected.getW(), actual.getW(), EPS);
+ Assert.assertEquals(msg, expected.getX(), actual.getX(), EPS);
+ Assert.assertEquals(msg, expected.getY(), actual.getY(), EPS);
+ Assert.assertEquals(msg, expected.getZ(), actual.getZ(), EPS);
+ }
}
----------------------------------------------------------------
This is an automated message from the Apache Git Service.
To respond to the message, please log on GitHub and use the
URL above to go to the specific comment.
For queries about this service, please contact Infrastructure at:
us...@infra.apache.org
With regards,
Apache Git Services
