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.Annotated;
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         Annotated 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).toAnnotated();
45             result = primer.prime(input, prediction, null, 0);
46             assertThat(result.azimuth).isEqualTo(input.azimuth);
47             assertThat(result.elevation).isEqualTo(input.elevation);
48             within = roll10.rotateVector(within);
49 
50             // Test outside
51             input = SphericalVector.fromCartesian(outside).toAnnotated();
52             result = primer.prime(input, prediction, null, 0);
53             assertThat(result.azimuth).isEqualTo(0);
54             assertThat(result.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         Annotated 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).toAnnotated();
67         result = primer.prime(input, prediction, null, 0);
68         // This is within FOV.
69         assertThat(result.azimuth).isEqualTo(input.azimuth);
70         assertThat(result.elevation).isEqualTo(input.elevation);
71 
72         input = SphericalVector.fromDegrees(45 + 180, 1, 10).toAnnotated();
73         result = primer.prime(input, prediction, null, 0);
74         // This is not within FOV.
75         assertThat(result.azimuth).isEqualTo(0);
76         assertThat(result.elevation).isEqualTo(0);
77         // Accuracy should be the size of the FOV blind spot.
78 
79         // Also test point at 0,0.
80         input = SphericalVector.fromDegrees(0, 0, 10).toAnnotated();
81         result = primer.prime(input, prediction, null, 0);
82         // This is within FOV.
83         assertThat(result.azimuth).isEqualTo(input.azimuth);
84         assertThat(result.elevation).isEqualTo(input.elevation);
85 
86         // Point at 90deg.
87         input = SphericalVector.fromDegrees(0, 90, 10).toAnnotated();
88         result = primer.prime(input, prediction, null, 0);
89         // This is not within FOV.
90         assertThat(result.azimuth).isEqualTo(0);
91         assertThat(result.elevation).isEqualTo(0);
92 
93         // Beyond maximum FOV
94         primer = new FovPrimer((float) toRadians(200));
95         // FOV is actually permitted behind "behind" the device too, test that.
96         input = SphericalVector.fromDegrees(35 + 180, 1, 10).toAnnotated();
97         result = primer.prime(input, prediction, null, 0);
98         // This is within FOV.
99         assertThat(result.azimuth).isEqualTo(input.azimuth);
100         assertThat(result.elevation).isEqualTo(input.elevation);
101 
102         // No prediction
103         primer = new FovPrimer((float) toRadians(10));
104         // Beyond the FOV, but no prediction data so it should go unchanged.
105         input = SphericalVector.fromDegrees(35, 1, 10).toAnnotated();
106         result = primer.prime(input, null, null, 0);
107         // This is within FOV.
108         assertThat(result.azimuth).isEqualTo(input.azimuth);
109         assertThat(result.elevation).isEqualTo(input.elevation);
110     }
111 }
112