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.math;
17 
18 import static com.android.server.uwb.correction.TestHelpers.assertClose;
19 
20 import static com.google.common.truth.Truth.assertThat;
21 
22 import static org.junit.Assert.assertEquals;
23 
24 import static java.lang.Math.toDegrees;
25 import static java.lang.Math.toRadians;
26 
27 import android.platform.test.annotations.Presubmit;
28 
29 import org.junit.Test;
30 
31 @Presubmit
32 public class SphericalVectorTest {
33     @Test
testCtorNormalization()34     public void testCtorNormalization() {
35         SphericalVector vec = SphericalVector.fromDegrees(185, 10, 10);
36         assertClose(toDegrees(vec.azimuth), -175);
37         assertClose(toDegrees(vec.elevation), 10);
38 
39         // This is looking right and up so far that you're basically looking
40         // at what's behind you on your left.
41         vec = SphericalVector.fromRadians((float) toRadians(5), (float) toRadians(110), 10);
42         assertClose(vec.azimuth, toRadians(-175)); // +5deg from "behind".
43 
44         // 20° beyond ⦨90° is ⦨70° because all elevations lead down from there.
45         assertClose(vec.elevation, toRadians(70));
46     }
47 
48     @Test
testToAoAVector()49     public void testToAoAVector() {
50         SphericalVector sv = SphericalVector.fromDegrees(0, 18, 10);
51         AoaVector av = sv.toAoAVector();
52 
53         // When az/el is at 0deg, aoav and sv are effectively the same.
54         assertThat(av.azimuth).isEqualTo(sv.azimuth);
55         assertThat(av.elevation).isEqualTo(sv.elevation);
56         assertThat(av.distance).isEqualTo(sv.distance);
57 
58         sv = SphericalVector.fromDegrees(-12, 0, 10);
59         av = sv.toAoAVector();
60         assertThat(av.azimuth).isEqualTo(sv.azimuth);
61         assertThat(av.elevation).isEqualTo(sv.elevation);
62         assertThat(av.distance).isEqualTo(sv.distance);
63 
64         assertClose(
65                 SphericalVector
66                         .fromDegrees(-35, -45, 1)
67                         .toCartesian()
68                         .length(),
69                 1);
70 
71         SphericalVector svec = SphericalVector.fromDegrees(15, 25, 6);
72         AoaVector avec = svec.toAoAVector();
73         assertClose(svec.toCartesian(), avec.toCartesian());
74 
75         svec = SphericalVector.fromDegrees(95, 25, 6);
76         avec = svec.toAoAVector();
77         assertClose(svec.toCartesian(), avec.toCartesian());
78         assertClose(avec.toSphericalVector().toCartesian(), avec.toCartesian());
79 
80         svec = SphericalVector.fromDegrees(-15, 35, 6);
81         avec = svec.toAoAVector();
82         assertClose(svec.toCartesian(), avec.toCartesian());
83         assertClose(avec.toSphericalVector().toCartesian(), avec.toCartesian());
84 
85         svec = SphericalVector.fromDegrees(-15, 35, 6);
86         avec = svec.toAoAVector();
87         assertClose(svec.toCartesian(), avec.toCartesian());
88         assertClose(avec.toSphericalVector().toCartesian(), avec.toCartesian());
89     }
90 
91     @Test
cartesian()92     public void cartesian() {
93         assertThat(SphericalVector.fromCartesian(0, 0, 0).distance).isEqualTo(0);
94 
95         // negative z-axis is "straight ahead".
96         assertClose(
97                 SphericalVector.fromCartesian(new Vector3(0, 0, -1)),
98                 SphericalVector.fromDegrees(0, 0, 1)
99         );
100 
101         // looking left.
102         assertClose(
103                 SphericalVector.fromCartesian(new Vector3(-1, 0, 0)),
104                 SphericalVector.fromDegrees(-90, 0, 1)
105         );
106 
107         // looking up.
108         SphericalVector gimbalLock = SphericalVector.fromCartesian(new Vector3(0, 1, 0));
109         // Note that this suffers from gimbal lock - meaning that ALL azimuth values are valid
110         // when looking up or down.
111         assertClose(toDegrees(gimbalLock.elevation), 90);
112         assertClose(gimbalLock.distance, 1);
113 
114         // looking 45 deg back left.
115         assertClose(
116                 SphericalVector.fromCartesian(new Vector3(-1, 0, 1)),
117                 SphericalVector.fromDegrees(-(90 + 45), 0, (float) Math.sqrt(2))
118         );
119 
120         Vector3 targ = new Vector3(-7, 23, 4);
121         assertClose(SphericalVector.fromCartesian(targ).toCartesian(), targ);
122     }
123 
124     @Test
annotated()125     public void annotated() {
126         SphericalVector vec = SphericalVector.fromDegrees(10, 15, 20);
127         SphericalVector.Annotated sparse = vec.toAnnotated();
128 
129         assertClose(sparse, vec);
130         assertThat(sparse.isComplete()).isTrue();
131         assertEquals("[⦡  10.0 100%,⦨ 15.0 100%,⤠20.00 100%]", sparse.toString());
132 
133         sparse = vec.toAnnotated(true, false, false);
134         assertThat(sparse.hasAzimuth).isTrue();
135         assertThat(sparse.hasElevation).isFalse();
136         assertThat(sparse.hasDistance).isFalse();
137         assertEquals("[⦡  10.0 100%,⦨  x  ,⤠  x  ]", sparse.toString());
138 
139         sparse = vec.toAnnotated(false, true, false);
140         assertThat(sparse.hasAzimuth).isFalse();
141         assertThat(sparse.hasElevation).isTrue();
142         assertThat(sparse.hasDistance).isFalse();
143         assertEquals("[⦡   x  ,⦨ 15.0 100%,⤠  x  ]", sparse.toString());
144 
145         sparse = vec.toAnnotated(false, false, true);
146         assertThat(sparse.hasAzimuth).isFalse();
147         assertThat(sparse.hasElevation).isFalse();
148         assertThat(sparse.hasDistance).isTrue();
149         assertEquals("[⦡   x  ,⦨  x  ,⤠20.00 100%]", sparse.toString());
150     }
151 
152     @Test
testToString()153     public void testToString() {
154         SphericalVector loc = SphericalVector.fromDegrees(1.2f, -3.4f, 5.6f);
155         assertEquals("[⦡   1.2,⦨ -3.4,⤠ 5.60]", loc.toString());
156     }
157 }
158