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.math.MathHelper.F_HALF_PI; 19 import static com.android.server.uwb.correction.math.MathHelper.F_PI; 20 21 import static java.lang.Math.abs; 22 import static java.lang.Math.acos; 23 import static java.lang.Math.asin; 24 import static java.lang.Math.cos; 25 import static java.lang.Math.max; 26 import static java.lang.Math.min; 27 import static java.lang.Math.signum; 28 import static java.lang.Math.sin; 29 import static java.lang.Math.sqrt; 30 import static java.lang.Math.toDegrees; 31 import static java.lang.Math.toRadians; 32 33 import android.util.Log; 34 35 import androidx.annotation.NonNull; 36 37 import com.android.internal.annotations.Immutable; 38 39 import java.util.Locale; 40 import java.util.Objects; 41 42 /** 43 * Represents a point in space as distance, azimuth and elevation. 44 * This uses OpenGL's right-handed coordinate system, where the origin is facing in the 45 * -Z direction. Increasing azimuth rotates around Y and increases X. Increasing 46 * elevation rotates around X and increases Y. 47 * 48 * Note that this is NOT quite a spherical vector. It represents angles seen by AoA antennas. 49 * In this implementation, azimuth and elevation are treated the same. Therefore, for example: 50 * Very "up" or "down" targets will have an azimuth near 0, because the signal will arrive at 51 * both AoA antennas at nearly the same time. 52 * In a spherical vector, azimuth is computed exclusively from the horizontal plane and treated 53 * independently of the vertical axis, but elevation is computed along the plane of the azimuth. 54 * This also means that there are some angles that are impossible. For example, something with 55 * a 90deg azimuth (directly right of the phone) cannot possibly be viewed by the elevation 56 * antennas from any angle other than 0deg. 57 */ 58 @Immutable 59 public final class AoaVector { 60 // If true, negative distances will be converted to a positive distance facing the opposite 61 // direction. 62 private static final boolean NORMALIZE_NEGATIVE_DISTANCE = false; 63 public static boolean logWarnings = false; 64 public final float distance; 65 public final float azimuth; 66 public final float elevation; 67 68 /** 69 * Creates a AoAVector from the azimuth, elevation and distance of a viewpoint that is 70 * facing into the -Z axis. Illegal azimuth and elevation combinations will be scaled away 71 * from +/-90deg such that they are legal. 72 * 73 * @param azimuth The angle along the X axis, around the Y axis. 74 * @param elevation The angle along the Y axis, around the X axis. 75 * @param distance The distance to the origin. 76 */ AoaVector(float azimuth, float elevation, float distance)77 private AoaVector(float azimuth, float elevation, float distance) { 78 elevation = MathHelper.normalizeRadians(elevation); 79 float ae = abs(elevation); 80 if (ae > F_HALF_PI) { 81 // Normalize elevation to be only +/-90 - if it's outside that, mirror and bound the 82 // elevation and flip the azimuth. 83 elevation = (F_PI - ae) * signum(elevation); 84 azimuth += F_PI; 85 } 86 if (NORMALIZE_NEGATIVE_DISTANCE && distance < 0) { 87 // Negative distance is equivalent to a flipped elevation and azimuth. 88 azimuth += F_PI; // turn 180deg. 89 elevation = -elevation; // Mirror top-to-bottom 90 distance = -distance; 91 } 92 azimuth = MathHelper.normalizeRadians(azimuth); 93 94 // Now verify validity 95 boolean backFacing = abs(azimuth) > F_HALF_PI; 96 97 // Compute azimuth if it was front-facing. 98 float laz = backFacing ? (F_PI * signum(azimuth) - azimuth) : azimuth; 99 float angleSum = abs(laz) + abs(elevation); 100 float scaleFactor = angleSum / (F_HALF_PI); 101 if (scaleFactor > 1) { 102 // The combination of degrees isn't possible - for example, the azimuth suggests that 103 // the target is exactly 90deg to the right, and yet elevation is non-zero. 104 // The elevation and azimuth will be scaled down until they are within 105 // legal limits. This will create a bias away from 90-degree readings. 106 // Note that azimuth will be corrected to higher than 90deg if it was originally 107 // above 90deg. 108 elevation /= scaleFactor; 109 azimuth = backFacing ? (F_PI * signum(azimuth) - laz / scaleFactor) : (azimuth 110 / scaleFactor); 111 if (logWarnings) { 112 Log.w("AOA", String.format( 113 "AoA value is illegal by a factor of %4.3f: ⦡% 3.1f,⦨% 3.1f", 114 scaleFactor, 115 toDegrees(azimuth), 116 toDegrees(elevation) 117 )); 118 } 119 } 120 121 this.distance = distance; 122 this.azimuth = azimuth; 123 this.elevation = elevation; 124 } 125 126 /** 127 * Converts the AoAVector to a spherical vector. 128 * @return An equivalent spherical vector. 129 */ toSphericalVector()130 public SphericalVector toSphericalVector() { 131 return SphericalVector.fromAoAVector(this); 132 } 133 134 /** 135 * Creates an AoAVector from azimuth and elevation in radians. 136 * 137 * @param azimuth The azimuth in radians. 138 * @param elevation The elevation in radians. 139 * @param distance The distance in meters. 140 * @return A new AoAVector. 141 */ 142 @NonNull fromRadians(float azimuth, float elevation, float distance)143 public static AoaVector fromRadians(float azimuth, float elevation, float distance) { 144 return new AoaVector(azimuth, elevation, distance); 145 } 146 147 /** 148 * Creates an AoAVector from azimuth and elevation in degrees. 149 * 150 * @param azimuth The azimuth in degrees. 151 * @param elevation The elevation in degrees. 152 * @param distance The distance in meters. 153 * @return A new AoAVector. 154 */ 155 @NonNull fromDegrees(float azimuth, float elevation, float distance)156 public static AoaVector fromDegrees(float azimuth, float elevation, float distance) { 157 return new AoaVector( 158 (float) toRadians(azimuth), 159 (float) toRadians(elevation), 160 distance); 161 } 162 163 /** 164 * Produces an AoA vector from a cartesian vector, converting X, Y and Z values to 165 * azimuth, elevation and distance. 166 * 167 * @param position The cartesian representation to convert. 168 * @return An equivalent AoA vector representation. 169 */ 170 @NonNull fromCartesian(@onNull Vector3 position)171 public static AoaVector fromCartesian(@NonNull Vector3 position) { 172 Objects.requireNonNull(position); 173 return fromCartesian(position.x, position.y, position.z); 174 } 175 176 /** 177 * Produces a AoA vector from a cartesian vector, converting X, Y and Z values to 178 * azimuth, elevation and distance. 179 * 180 * @param x The cartesian x-coordinate to convert. 181 * @param y The cartesian y-coordinate to convert. 182 * @param z The cartesian z-coordinate to convert. 183 * @return An equivalent AoA vector representation. 184 */ 185 @NonNull fromCartesian(float x, float y, float z)186 public static AoaVector fromCartesian(float x, float y, float z) { 187 float d = (float) sqrt(x * x + y * y + z * z); 188 if (d == 0) { 189 return new AoaVector(0, 0, 0); 190 } 191 float azimuth = (float) asin(min(max(x / d, -1), 1)); 192 float elevation = (float) asin(min(max(y / d, -1), 1)); 193 if (z > 0) { 194 // If z is "behind", mirror azimuth front/back. 195 azimuth = F_PI * signum(azimuth) - azimuth; 196 } 197 return new AoaVector(azimuth, elevation, d); 198 } 199 200 /** 201 * Converts a SphericalVector to an AoAVector. 202 * @param vec The SphericalVector to convert. 203 * @return An equivalent AoAVector. 204 */ fromSphericalVector(SphericalVector vec)205 public static AoaVector fromSphericalVector(SphericalVector vec) { 206 float azimuth = vec.azimuth; 207 boolean mirrored = abs(azimuth) > F_HALF_PI; 208 if (mirrored) { 209 azimuth = F_PI - azimuth; 210 } 211 double ca = cos(azimuth); 212 double se = sin(vec.elevation); 213 double ce = cos(vec.elevation); 214 double az = acos(sqrt(max(ce * ce * ca * ca, 0) + se * se)) 215 * signum(vec.azimuth); 216 if (mirrored) { 217 return new AoaVector(F_PI - (float) az, vec.elevation, vec.distance); 218 } else { 219 return new AoaVector((float) az, vec.elevation, vec.distance); 220 } 221 } 222 223 /** 224 * Converts to a Vector3. 225 * See {@link #AoaVector} for orientation information. 226 * 227 * @return A Vector3 whose coordinates are at the indicated location. 228 */ 229 @NonNull toCartesian()230 public Vector3 toCartesian() { 231 float x = distance * (float) sin(azimuth); 232 float y = distance * (float) sin(elevation); 233 float z = (float) sqrt(distance * distance - x * x - y * y); 234 if (Float.isNaN(z)) { 235 z = 0; // Impossible angle. This is the closest we can get to it. 236 } 237 if (abs(azimuth) < F_HALF_PI) { 238 z = -z; 239 } 240 return new Vector3(x, y, z); 241 } 242 243 /** 244 * {@inheritDoc} 245 */ 246 @NonNull 247 @Override toString()248 public String toString() { 249 String format = "[⦡% 6.1f,⦨% 5.1f,⤠%5.2f]"; 250 return String.format( 251 Locale.getDefault(), 252 format, 253 toDegrees(azimuth), 254 toDegrees(elevation), 255 distance 256 ); 257 } 258 } 259