1 /* 2 * Copyright (C) 2023 The Android Open Source Project 3 * 4 * Licensed under the Apache License, Version 2.0 (the "License"); 5 * you may not use this file except in compliance with the License. 6 * You may obtain a copy of the License at 7 * 8 * http://www.apache.org/licenses/LICENSE-2.0 9 * 10 * Unless required by applicable law or agreed to in writing, software 11 * distributed under the License is distributed on an "AS IS" BASIS, 12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 * See the License for the specific language governing permissions and 14 * limitations under the License. 15 */ 16 17 package com.android.uwb.fusion.math; 18 19 import static com.android.uwb.fusion.TestHelpers.assertClose; 20 import static com.android.uwb.fusion.math.MathHelper.F_HALF_PI; 21 22 import static org.junit.Assert.assertTrue; 23 24 import static java.lang.Math.abs; 25 26 import android.platform.test.annotations.Presubmit; 27 28 import org.junit.Test; 29 30 @Presubmit 31 public class QuaternionTest { 32 33 @Test testYawPitchRoll()34 public void testYawPitchRoll() { 35 Quaternion quaternion = Quaternion.yawPitchRoll(0.5f, 1f, 1.5f); 36 Vector3 ypr = quaternion.toYawPitchRoll(); 37 38 assertTrue(abs(ypr.x - 0.5) < 0.001); 39 assertTrue(abs(ypr.y - 1) < 0.001); 40 assertTrue(abs(ypr.z - 1.5) < 0.001); 41 42 // See the Javadoc for the 'Quaternion' class for an explanation of how these 43 // results are determined. 44 45 quaternion = Quaternion.yawPitchRoll(F_HALF_PI, 0, 0); 46 assertClose(quaternion.rotateVector(new Vector3(1, 2, 3)), new Vector3(3, 2, -1)); 47 48 quaternion = Quaternion.yawPitchRoll(0, F_HALF_PI, 0); 49 assertClose(quaternion.rotateVector(new Vector3(1, 2, 3)), new Vector3(1, -3, 2)); 50 51 quaternion = Quaternion.yawPitchRoll(0, 0, F_HALF_PI); 52 assertClose(quaternion.rotateVector(new Vector3(1, 2, 3)), new Vector3(-2, 1, 3)); 53 } 54 } 55