001// Copyright (c) FIRST and other WPILib contributors. 002// Open Source Software; you can modify and/or share it under the terms of 003// the WPILib BSD license file in the root directory of this project. 004 005package edu.wpi.first.math; 006 007import edu.wpi.first.math.geometry.Translation2d; 008import edu.wpi.first.math.geometry.Translation3d; 009 010/** Math utility functions. */ 011public final class MathUtil { 012 private MathUtil() { 013 throw new AssertionError("utility class"); 014 } 015 016 /** 017 * Returns value clamped between low and high boundaries. 018 * 019 * @param value Value to clamp. 020 * @param low The lower boundary to which to clamp value. 021 * @param high The higher boundary to which to clamp value. 022 * @return The clamped value. 023 */ 024 public static int clamp(int value, int low, int high) { 025 return Math.max(low, Math.min(value, high)); 026 } 027 028 /** 029 * Returns value clamped between low and high boundaries. 030 * 031 * @param value Value to clamp. 032 * @param low The lower boundary to which to clamp value. 033 * @param high The higher boundary to which to clamp value. 034 * @return The clamped value. 035 */ 036 public static double clamp(double value, double low, double high) { 037 return Math.max(low, Math.min(value, high)); 038 } 039 040 /** 041 * Returns 0.0 if the given value is within the specified range around zero. The remaining range 042 * between the deadband and the maximum magnitude is scaled from 0.0 to the maximum magnitude. 043 * 044 * @param value Value to clip. 045 * @param deadband Range around zero. 046 * @param maxMagnitude The maximum magnitude of the input. Can be infinite. 047 * @return The value after the deadband is applied. 048 */ 049 public static double applyDeadband(double value, double deadband, double maxMagnitude) { 050 if (Math.abs(value) < deadband) { 051 return 0; 052 } 053 if (value > 0.0) { 054 // Map deadband to 0 and map max to max with a linear relationship. 055 // 056 // y - y₁ = m(x - x₁) 057 // y - y₁ = (y₂ - y₁)/(x₂ - x₁) (x - x₁) 058 // y = (y₂ - y₁)/(x₂ - x₁) (x - x₁) + y₁ 059 // 060 // (x₁, y₁) = (deadband, 0) and (x₂, y₂) = (max, max). 061 // 062 // x₁ = deadband 063 // y₁ = 0 064 // x₂ = max 065 // y₂ = max 066 // y = (max - 0)/(max - deadband) (x - deadband) + 0 067 // y = max/(max - deadband) (x - deadband) 068 // 069 // To handle high values of max, rewrite so that max only appears on the denominator. 070 // 071 // y = ((max - deadband) + deadband)/(max - deadband) (x - deadband) 072 // y = (1 + deadband/(max - deadband)) (x - deadband) 073 return (1 + deadband / (maxMagnitude - deadband)) * (value - deadband); 074 } else { 075 // Map -deadband to 0 and map -max to -max with a linear relationship. 076 // 077 // y - y₁ = m(x - x₁) 078 // y - y₁ = (y₂ - y₁)/(x₂ - x₁) (x - x₁) 079 // y = (y₂ - y₁)/(x₂ - x₁) (x - x₁) + y₁ 080 // 081 // (x₁, y₁) = (-deadband, 0) and (x₂, y₂) = (-max, -max). 082 // 083 // x₁ = -deadband 084 // y₁ = 0 085 // x₂ = -max 086 // y₂ = -max 087 // y = (-max - 0)/(-max + deadband) (x + deadband) + 0 088 // y = max/(max - deadband) (x + deadband) 089 // 090 // To handle high values of max, rewrite so that max only appears on the denominator. 091 // 092 // y = ((max - deadband) + deadband)/(max - deadband) (x + deadband) 093 // y = (1 + deadband/(max - deadband)) (x + deadband) 094 return (1 + deadband / (maxMagnitude - deadband)) * (value + deadband); 095 } 096 } 097 098 /** 099 * Returns 0.0 if the given value is within the specified range around zero. The remaining range 100 * between the deadband and 1.0 is scaled from 0.0 to 1.0. 101 * 102 * @param value Value to clip. 103 * @param deadband Range around zero. 104 * @return The value after the deadband is applied. 105 */ 106 public static double applyDeadband(double value, double deadband) { 107 return applyDeadband(value, deadband, 1); 108 } 109 110 /** 111 * Raises the input to the power of the given exponent while preserving its sign. 112 * 113 * <p>The function normalizes the input value to the range [0, 1] based on the maximum magnitude, 114 * raises it to the power of the exponent, then scales the result back to the original range and 115 * copying the sign. This keeps the value in the original range and gives consistent curve 116 * behavior regardless of the input value's scale. 117 * 118 * <p>This is useful for applying smoother or more aggressive control response curves (e.g. 119 * joystick input shaping). 120 * 121 * @param value The input value to transform. 122 * @param exponent The exponent to apply (e.g. 1.0 = linear, 2.0 = squared curve). Must be 123 * positive. 124 * @param maxMagnitude The maximum expected absolute value of input. Must be positive. 125 * @return The transformed value with the same sign and scaled to the input range. 126 */ 127 public static double copySignPow(double value, double exponent, double maxMagnitude) { 128 return Math.copySign(Math.pow(Math.abs(value) / maxMagnitude, exponent), value) * maxMagnitude; 129 } 130 131 /** 132 * Raises the input to the power of the given exponent while preserving its sign. 133 * 134 * <p>This is useful for applying smoother or more aggressive control response curves (e.g. 135 * joystick input shaping). 136 * 137 * @param value The input value to transform. 138 * @param exponent The exponent to apply (e.g. 1.0 = linear, 2.0 = squared curve). Must be 139 * positive. 140 * @return The transformed value with the same sign. 141 */ 142 public static double copySignPow(double value, double exponent) { 143 return copySignPow(value, exponent, 1); 144 } 145 146 /** 147 * Returns modulus of input. 148 * 149 * @param input Input value to wrap. 150 * @param minimumInput The minimum value expected from the input. 151 * @param maximumInput The maximum value expected from the input. 152 * @return The wrapped value. 153 */ 154 public static double inputModulus(double input, double minimumInput, double maximumInput) { 155 double modulus = maximumInput - minimumInput; 156 157 // Wrap input if it's above the maximum input 158 int numMax = (int) ((input - minimumInput) / modulus); 159 input -= numMax * modulus; 160 161 // Wrap input if it's below the minimum input 162 int numMin = (int) ((input - maximumInput) / modulus); 163 input -= numMin * modulus; 164 165 return input; 166 } 167 168 /** 169 * Wraps an angle to the range -π to π radians. 170 * 171 * @param angleRadians Angle to wrap in radians. 172 * @return The wrapped angle. 173 */ 174 public static double angleModulus(double angleRadians) { 175 return inputModulus(angleRadians, -Math.PI, Math.PI); 176 } 177 178 /** 179 * Perform linear interpolation between two values. 180 * 181 * @param startValue The value to start at. 182 * @param endValue The value to end at. 183 * @param t How far between the two values to interpolate. This is clamped to [0, 1]. 184 * @return The interpolated value. 185 */ 186 public static double interpolate(double startValue, double endValue, double t) { 187 return startValue + (endValue - startValue) * MathUtil.clamp(t, 0, 1); 188 } 189 190 /** 191 * Return where within interpolation range [0, 1] q is between startValue and endValue. 192 * 193 * @param startValue Lower part of interpolation range. 194 * @param endValue Upper part of interpolation range. 195 * @param q Query. 196 * @return Interpolant in range [0, 1]. 197 */ 198 public static double inverseInterpolate(double startValue, double endValue, double q) { 199 double totalRange = endValue - startValue; 200 if (totalRange <= 0) { 201 return 0.0; 202 } 203 double queryToStart = q - startValue; 204 if (queryToStart <= 0) { 205 return 0.0; 206 } 207 return queryToStart / totalRange; 208 } 209 210 /** 211 * Checks if the given value matches an expected value within a certain tolerance. 212 * 213 * @param expected The expected value 214 * @param actual The actual value 215 * @param tolerance The allowed difference between the actual and the expected value 216 * @return Whether or not the actual value is within the allowed tolerance 217 */ 218 public static boolean isNear(double expected, double actual, double tolerance) { 219 if (tolerance < 0) { 220 throw new IllegalArgumentException("Tolerance must be a non-negative number!"); 221 } 222 return Math.abs(expected - actual) < tolerance; 223 } 224 225 /** 226 * Checks if the given value matches an expected value within a certain tolerance. Supports 227 * continuous input for cases like absolute encoders. 228 * 229 * <p>Continuous input means that the min and max value are considered to be the same point, and 230 * tolerances can be checked across them. A common example would be for absolute encoders: calling 231 * isNear(2, 359, 5, 0, 360) returns true because 359 is 1 away from 360 (which is treated as the 232 * same as 0) and 2 is 2 away from 0, adding up to an error of 3 degrees, which is within the 233 * given tolerance of 5. 234 * 235 * @param expected The expected value 236 * @param actual The actual value 237 * @param tolerance The allowed difference between the actual and the expected value 238 * @param min Smallest value before wrapping around to the largest value 239 * @param max Largest value before wrapping around to the smallest value 240 * @return Whether or not the actual value is within the allowed tolerance 241 */ 242 public static boolean isNear( 243 double expected, double actual, double tolerance, double min, double max) { 244 if (tolerance < 0) { 245 throw new IllegalArgumentException("Tolerance must be a non-negative number!"); 246 } 247 // Max error is exactly halfway between the min and max 248 double errorBound = (max - min) / 2.0; 249 double error = MathUtil.inputModulus(expected - actual, -errorBound, errorBound); 250 return Math.abs(error) < tolerance; 251 } 252 253 /** 254 * Limits translation velocity. 255 * 256 * @param current Translation at current timestep. 257 * @param next Translation at next timestep. 258 * @param dt Timestep duration. 259 * @param maxVelocity Maximum translation velocity. 260 * @return Returns the next Translation2d limited to maxVelocity 261 */ 262 public static Translation2d slewRateLimit( 263 Translation2d current, Translation2d next, double dt, double maxVelocity) { 264 if (maxVelocity < 0) { 265 Exception e = new IllegalArgumentException(); 266 MathSharedStore.reportError( 267 "maxVelocity must be a non-negative number, got " + maxVelocity, e.getStackTrace()); 268 return next; 269 } 270 Translation2d diff = next.minus(current); 271 double dist = diff.getNorm(); 272 if (dist < 1e-9) { 273 return next; 274 } 275 if (dist > maxVelocity * dt) { 276 // Move maximum allowed amount in direction of the difference 277 return current.plus(diff.times(maxVelocity * dt / dist)); 278 } 279 return next; 280 } 281 282 /** 283 * Limits translation velocity. 284 * 285 * @param current Translation at current timestep. 286 * @param next Translation at next timestep. 287 * @param dt Timestep duration. 288 * @param maxVelocity Maximum translation velocity. 289 * @return Returns the next Translation3d limited to maxVelocity 290 */ 291 public static Translation3d slewRateLimit( 292 Translation3d current, Translation3d next, double dt, double maxVelocity) { 293 if (maxVelocity < 0) { 294 Exception e = new IllegalArgumentException(); 295 MathSharedStore.reportError( 296 "maxVelocity must be a non-negative number, got " + maxVelocity, e.getStackTrace()); 297 return next; 298 } 299 Translation3d diff = next.minus(current); 300 double dist = diff.getNorm(); 301 if (dist < 1e-9) { 302 return next; 303 } 304 if (dist > maxVelocity * dt) { 305 // Move maximum allowed amount in direction of the difference 306 return current.plus(diff.times(maxVelocity * dt / dist)); 307 } 308 return next; 309 } 310}