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 package com.android.server.uwb.correction.primers; 17 18 import static com.google.common.truth.Truth.assertThat; 19 20 import static java.lang.Math.toRadians; 21 22 import com.android.server.uwb.correction.math.Quaternion; 23 import com.android.server.uwb.correction.math.SphericalVector; 24 import com.android.server.uwb.correction.math.SphericalVector.Sparse; 25 import com.android.server.uwb.correction.math.Vector3; 26 27 import org.junit.Test; 28 29 public class FoVPrimerTest { 30 @Test conversionTest()31 public void conversionTest() { 32 FovPrimer primer = new FovPrimer((float) toRadians(45)); 33 Sparse input, result; 34 SphericalVector prediction = SphericalVector.fromDegrees(0, 0, 0); 35 36 // The FOV formula reduced to az+el<fov, which seems too simple to be real. 37 // To test this, I'll place a point one degree with the FOV, and one outside the FOV, 38 // then "roll" and test those points in increments all the way around the perimeter. 39 Quaternion roll10 = Quaternion.yawPitchRoll(0, 0, (float) toRadians(10)); 40 Vector3 within = SphericalVector.fromDegrees(44, 0, 10).toCartesian(); 41 Vector3 outside = SphericalVector.fromDegrees(46, 0, 10).toCartesian(); 42 for (int x = 0; x < 36; x++) { 43 // Test within 44 input = SphericalVector.fromCartesian(within).toSparse(); 45 result = primer.prime(input, prediction, null, 0); 46 assertThat(result.vector.azimuth).isEqualTo(input.vector.azimuth); 47 assertThat(result.vector.elevation).isEqualTo(input.vector.elevation); 48 within = roll10.rotateVector(within); 49 50 // Test outside 51 input = SphericalVector.fromCartesian(outside).toSparse(); 52 result = primer.prime(input, prediction, null, 0); 53 assertThat(result.vector.azimuth).isEqualTo(0); 54 assertThat(result.vector.elevation).isEqualTo(0); 55 outside = roll10.rotateVector(outside); 56 } 57 } 58 59 @Test edgeCases()60 public void edgeCases() { 61 FovPrimer primer = new FovPrimer((float) toRadians(45)); 62 Sparse input, result; 63 SphericalVector prediction = SphericalVector.fromDegrees(0, 0, 0); 64 65 // FOV is actually permitted behind "behind" the device too, test that. 66 input = SphericalVector.fromDegrees(35 + 180, 1, 10).toSparse(); 67 result = primer.prime(input, prediction, null, 0); 68 // This is within FOV. 69 assertThat(result.vector.azimuth).isEqualTo(input.vector.azimuth); 70 assertThat(result.vector.elevation).isEqualTo(input.vector.elevation); 71 72 input = SphericalVector.fromDegrees(45 + 180, 1, 10).toSparse(); 73 result = primer.prime(input, prediction, null, 0); 74 // This is not within FOV. 75 assertThat(result.vector.azimuth).isEqualTo(0); 76 assertThat(result.vector.elevation).isEqualTo(0); 77 78 // Also test point at 0,0. 79 input = SphericalVector.fromDegrees(0, 0, 10).toSparse(); 80 result = primer.prime(input, prediction, null, 0); 81 // This is within FOV. 82 assertThat(result.vector.azimuth).isEqualTo(input.vector.azimuth); 83 assertThat(result.vector.elevation).isEqualTo(input.vector.elevation); 84 85 // Point at 90deg. 86 input = SphericalVector.fromDegrees(0, 90, 10).toSparse(); 87 result = primer.prime(input, prediction, null, 0); 88 // This is not within FOV. 89 assertThat(result.vector.azimuth).isEqualTo(0); 90 assertThat(result.vector.elevation).isEqualTo(0); 91 92 // Beyond maximum FOV 93 primer = new FovPrimer((float) toRadians(200)); 94 // FOV is actually permitted behind "behind" the device too, test that. 95 input = SphericalVector.fromDegrees(35 + 180, 1, 10).toSparse(); 96 result = primer.prime(input, prediction, null, 0); 97 // This is within FOV. 98 assertThat(result.vector.azimuth).isEqualTo(input.vector.azimuth); 99 assertThat(result.vector.elevation).isEqualTo(input.vector.elevation); 100 101 // No prediction 102 primer = new FovPrimer((float) toRadians(10)); 103 // Beyond the FOV, but no prediction data so it should go unchanged. 104 input = SphericalVector.fromDegrees(35, 1, 10).toSparse(); 105 result = primer.prime(input, null, null, 0); 106 // This is within FOV. 107 assertThat(result.vector.azimuth).isEqualTo(input.vector.azimuth); 108 assertThat(result.vector.elevation).isEqualTo(input.vector.elevation); 109 } 110 } 111