| /* |
| * 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.numbers.quaternion; |
| |
| import org.apache.commons.numbers.core.Precision; |
| |
| import org.junit.jupiter.api.Assertions; |
| import org.junit.jupiter.api.Test; |
| |
| public class SlerpTest { |
| |
| 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 |
| Assertions.assertEquals(1.0, result.norm(), EPS); |
| Assertions.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 |
| Assertions.assertEquals(1.0, result.norm(), EPS); |
| Assertions.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 |
| Assertions.assertEquals(-1.0, q1.dot(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 |
| Assertions.assertArrayEquals(new double[] { 2, 0, 1 }, |
| transformVector(slerp.apply(0), vec), EPS); |
| |
| Assertions.assertArrayEquals(new double[] { SQRT_2, SQRT_2, 1 }, |
| transformVector(slerp.apply(0.25), vec), EPS); |
| |
| Assertions.assertArrayEquals(new double[] { 0, 2, 1 }, |
| transformVector(slerp.apply(0.5), vec), EPS); |
| |
| Assertions.assertArrayEquals(new double[] { -SQRT_2, SQRT_2, 1 }, |
| transformVector(slerp.apply(0.75), vec), EPS); |
| |
| Assertions.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 |
| Assertions.assertArrayEquals(startVec, transformVector(slerp.apply(0), vec), EPS); |
| Assertions.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 |
| Assertions.assertEquals(vecNorm, norm(slerpVec), EPS); |
| |
| // make sure that we're steadily progressing to the end angle |
| double angle = angle(slerpVec, startVec); |
| Assertions.assertTrue(Precision.compareTo(angle, prevAngle, EPS) >= 0, |
| "Expected slerp angle to continuously increase; previous angle was " + |
| prevAngle + " and new angle is " + angle); |
| } |
| } |
| |
| @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); |
| Assertions.assertArrayEquals(new double[] { 1, 0, 0 }, transformVector(slerp12.apply(-4.5), vec), EPS); |
| Assertions.assertArrayEquals(new double[] { 1, 0, 0 }, transformVector(slerp12.apply(-0.5), vec), EPS); |
| Assertions.assertArrayEquals(new double[] { -1, 0, 0 }, transformVector(slerp12.apply(1.5), vec), EPS); |
| Assertions.assertArrayEquals(new double[] { -1, 0, 0 }, transformVector(slerp12.apply(5.5), vec), EPS); |
| |
| Slerp slerp21 = new Slerp(q2, q1); |
| Assertions.assertArrayEquals(new double[] { -1, 0, 0 }, transformVector(slerp21.apply(-4.5), vec), EPS); |
| Assertions.assertArrayEquals(new double[] { -1, 0, 0 }, transformVector(slerp21.apply(-0.5), vec), EPS); |
| Assertions.assertArrayEquals(new double[] { 1, 0, 0 }, transformVector(slerp21.apply(1.5), vec), EPS); |
| Assertions.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; |
| |
| Assertions.assertEquals(expected.getW(), actual.getW(), EPS, msg); |
| Assertions.assertEquals(expected.getX(), actual.getX(), EPS, msg); |
| Assertions.assertEquals(expected.getY(), actual.getY(), EPS, msg); |
| Assertions.assertEquals(expected.getZ(), actual.getZ(), EPS, msg); |
| } |
| } |