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.PI; 19 import static java.lang.Math.abs; 20 import static java.lang.Math.ceil; 21 import static java.lang.Math.max; 22 import static java.lang.Math.min; 23 import static java.lang.Math.sqrt; 24 25 /** Static functions for common math operations. */ 26 public final class MathHelper { 27 public static final float F_PI = (float) PI; 28 public static final float F_HALF_PI = (float) (PI / 2); 29 public static final long MS_PER_SEC = 1000; 30 private static final float RSQRT_THRESHOLD = 0.0002821f; 31 32 /** Clamps a value between a minimum and maximum range. */ clamp(float value, float min, float max)33 public static float clamp(float value, float min, float max) { 34 return min(max, max(min, value)); 35 } 36 37 /** 38 * Linearly interpolates between a and b by a ratio. 39 * 40 * @param a the beginning value 41 * @param b the ending value 42 * @param t ratio between the two floats 43 * @return interpolated value between the two floats 44 */ lerp(float a, float b, float t)45 public static float lerp(float a, float b, float t) { 46 return a + t * (b - a); 47 } 48 49 /** Calculates the 1 / sqrt(x), with a maximum error of 1.5 ulps. */ rsqrt(float x)50 public static float rsqrt(float x) { 51 return abs(x - 1) < RSQRT_THRESHOLD 52 ? 2 / (1 + x) // 1st order Padé approximant for inverse square root. 53 : 1 / (float) sqrt(x); 54 } 55 56 /** 57 * Converts degrees that may be outside +/-180 to an equivalent rotation value between 58 * -180 (excl) and 180 (incl). 59 * @param deg The degrees to normalize 60 * @return A value above -180 and up to 180 that has an equivalent angle to the input. 61 */ normalizeDegrees(float deg)62 public static float normalizeDegrees(float deg) { 63 return deg - 360 * (float) ceil((deg - 180) / 360); 64 } 65 66 /** 67 * Converts radians that may be outside +/-PI to an equivalent rotation value. 68 * @param rad The radians to normalize 69 * @return A value above -PI and up to pi that has an equivalent angle to the input. 70 */ normalizeRadians(float rad)71 public static float normalizeRadians(float rad) { 72 return rad - (F_PI * 2) * (float) ceil((rad - F_PI) / (F_PI * 2)); 73 } 74 MathHelper()75 private MathHelper() {} 76 } 77