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 java.lang.Math.acos; 19 import static java.lang.Math.sqrt; 20 21 import androidx.annotation.NonNull; 22 23 import com.android.internal.annotations.Immutable; 24 25 import java.util.Locale; 26 27 /** 28 * A Vector with 3 floats. This class is immutable. 29 */ 30 @Immutable 31 public class Vector3 { 32 public static final Vector3 ORIGIN = new Vector3(); 33 34 public final float x; 35 public final float y; 36 public final float z; 37 38 /** 39 * Creates a vector at the origin. Because a Vector3 is immutable, it's better to use the 40 * singleton of {@link #ORIGIN} 41 */ Vector3()42 private Vector3() { 43 x = 0; 44 y = 0; 45 z = 0; 46 } 47 48 /** 49 * Creates a new Vector3. 50 * 51 * @param x The x value of the vector. 52 * @param y The y value of the vector. 53 * @param z The z value of the vector. 54 */ Vector3(float x, float y, float z)55 public Vector3(float x, float y, float z) { 56 this.x = x; 57 this.y = y; 58 this.z = z; 59 } 60 61 @NonNull 62 @Override toString()63 public String toString() { 64 return String.format(Locale.getDefault(), "[% 5.1f,% 5.1f,% 5.1f]", x, y, z); 65 } 66 67 /** Get a new Vector3 scaled to the unit length. */ normalized()68 public Vector3 normalized() { 69 Vector3 result; 70 float normSquared = lengthSquared(); 71 72 if (normSquared <= 0.0f) { 73 result = ORIGIN; 74 } else { 75 float norm = MathHelper.rsqrt(normSquared); 76 result = new Vector3(x * norm, y * norm, z * norm); 77 } 78 return result; 79 } 80 81 /** Get a new Vector3 multiplied by a scalar amount. */ 82 @NonNull scaled(float a)83 public Vector3 scaled(float a) { 84 return new Vector3(x * a, y * a, z * a); 85 } 86 87 /** Adds this vector to another and returns the sum. */ 88 @NonNull add(@onNull Vector3 rhs)89 public Vector3 add(@NonNull Vector3 rhs) { 90 return new Vector3(x + rhs.x, y + rhs.y, z + rhs.z); 91 } 92 93 /** 94 * Subtracts a vector from this vector. 95 * @param rhs The subtrahend. 96 * @return The difference between the two vectors. 97 */ 98 @NonNull subtract(@onNull Vector3 rhs)99 public Vector3 subtract(@NonNull Vector3 rhs) { 100 return new Vector3(x - rhs.x, y - rhs.y, z - rhs.z); 101 } 102 103 /** 104 * Multiplies each element of two vectors 105 */ 106 @NonNull multiply(@onNull Vector3 rhs)107 public Vector3 multiply(@NonNull Vector3 rhs) { 108 return new Vector3(x * rhs.x, y * rhs.y, z * rhs.z); 109 } 110 111 /** Get dot product of two Vector3s. */ dot(@onNull Vector3 lhs, @NonNull Vector3 rhs)112 public static float dot(@NonNull Vector3 lhs, @NonNull Vector3 rhs) { 113 return lhs.x * rhs.x + lhs.y * rhs.y + lhs.z * rhs.z; 114 } 115 116 /** Get cross product of two Vector3s. */ 117 @NonNull cross(@onNull Vector3 lhs, @NonNull Vector3 rhs)118 public static Vector3 cross(@NonNull Vector3 lhs, @NonNull Vector3 rhs) { 119 float lhsX = lhs.x; 120 float lhsY = lhs.y; 121 float lhsZ = lhs.z; 122 float rhsX = rhs.x; 123 float rhsY = rhs.y; 124 float rhsZ = rhs.z; 125 return new Vector3( 126 lhsY * rhsZ - lhsZ * rhsY, 127 lhsZ * rhsX - lhsX * rhsZ, 128 lhsX * rhsY - lhsY * rhsX 129 ); 130 } 131 132 /** 133 * Gets the square of the length of the vector. When performing length comparisons, 134 * it is more optimal to compare against a squared length to avoid having to perform 135 * a sqrt. 136 * @return The square of the length of the vector. 137 */ lengthSquared()138 public float lengthSquared() { 139 return x * x + y * y + z * z; 140 } 141 142 /** 143 * Gets the length of the vector. Consider {@link Vector3#lengthSquared} for performance. 144 * 145 * @return The length of the vector. 146 */ length()147 public float length() { 148 return (float) sqrt(lengthSquared()); 149 } 150 151 /** 152 * Returns a clamped version of the current Vector. 153 * 154 * @param min the floor value, each individual component if below will be replaced by this. 155 * @param max the ceiling value, each individual component if above will be replaced by this. 156 * @return A Vector3 within the provided range. 157 */ clamp(@onNull Vector3 min, @NonNull Vector3 max)158 public Vector3 clamp(@NonNull Vector3 min, @NonNull Vector3 max) { 159 float clampedX = MathHelper.clamp(x, min.x, max.x); 160 float clampedY = MathHelper.clamp(y, min.y, max.y); 161 float clampedZ = MathHelper.clamp(z, min.z, max.z); 162 163 return new Vector3(clampedX, clampedY, clampedZ); 164 } 165 166 /** 167 * Get the shortest angle in radians between two vectors. The result is never greater than PI 168 * radians (180 degrees). 169 */ angleBetweenVectors(@onNull Vector3 a, @NonNull Vector3 b)170 public static float angleBetweenVectors(@NonNull Vector3 a, @NonNull Vector3 b) { 171 float lengthA = a.length(); 172 float lengthB = b.length(); 173 float combinedLength = lengthA * lengthB; 174 175 if (combinedLength < 1e-10f) { 176 return 0.0f; 177 } 178 179 float dot = Vector3.dot(a, b); 180 float cos = dot / combinedLength; 181 182 // Clamp due to floating point precision that could cause dot to be > combinedLength. 183 // Which would cause acos to return NaN. 184 cos = MathHelper.clamp(cos, -1.0f, 1.0f); 185 return (float) acos(cos); 186 } 187 188 /** 189 * Linearly interpolates between two points. Interpolates between the points a and b by the 190 * interpolant alpha. This is most commonly used to find a point some fraction of the way along 191 * a line between two endpoints (e.g. to move an object gradually between those points). This 192 * is an extension of {@link MathHelper#lerp(float, float, float)} for Vector3. 193 * 194 * @param start the beginning Vector3 (variable a in MathHelper#lerp). 195 * @param end the ending Vector3 (variable b in MathHelper#lerp). 196 * @param ratio ratio between the two Vector3 (variable t in MathHelper#lerp). 197 * @return interpolated value between the two Vector3 198 */ 199 @NonNull lerp(@onNull Vector3 start, @NonNull Vector3 end, float ratio)200 public static Vector3 lerp(@NonNull Vector3 start, @NonNull Vector3 end, float ratio) { 201 return new Vector3( 202 /*x=*/ MathHelper.lerp(start.x, end.x, ratio), 203 /*y=*/ MathHelper.lerp(start.y, end.y, ratio), 204 /*z=*/ MathHelper.lerp(start.z, end.z, ratio)); 205 } 206 207 /** 208 * Converts a vector expressed in radians (ie - yaw, pitch, roll), into degrees. Primarily 209 * used as a convenience to display data to a user. 210 * @return A Vector3 multiplied by 180/PI. 211 */ toDegrees()212 public Vector3 toDegrees() { 213 return new Vector3( 214 (float) Math.toDegrees(x), 215 (float) Math.toDegrees(y), 216 (float) Math.toDegrees(z) 217 ); 218 } 219 220 /** 221 * Negates all values in the vector and returns the result. 222 * 223 * @return A negated vector. 224 */ 225 @NonNull inverted()226 public Vector3 inverted() { 227 return new Vector3(-x, -y, -z); 228 } 229 } 230