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}