Package edu.wpi.first.math

Class MathUtil

java.lang.Object
edu.wpi.first.math.MathUtil
public final class MathUtil extends Object
Math utility functions.

  • Method Summary

    Modifier and Type
    Method
    Description
    static double
    angleModulus(double angleRadians)
    Wraps an angle to the range -π to π radians.
    static double
    applyDeadband(double value, double deadband)
    Returns 0.0 if the given value is within the specified range around zero.
    static double
    applyDeadband(double value, double deadband, double maxMagnitude)
    Returns 0.0 if the given value is within the specified range around zero.
    static <R extends Num>
    Vector<R>
    applyDeadband(Vector<R> value, double deadband)
    Returns a zero vector if the given vector is within the specified distance from the origin.
    static <R extends Num>
    Vector<R>
    applyDeadband(Vector<R> value, double deadband, double maxMagnitude)
    Returns a zero vector if the given vector is within the specified distance from the origin.
    static double
    copyDirectionPow(double value, double exponent)
    Raises the input to the power of the given exponent while preserving its sign.
    static double
    copyDirectionPow(double value, double exponent, double maxMagnitude)
    Raises the input to the power of the given exponent while preserving its sign.
    static <R extends Num>
    Vector<R>
    copyDirectionPow(Vector<R> value, double exponent)
    Raises the norm of the input to the power of the given exponent while preserving its direction.
    static <R extends Num>
    Vector<R>
    copyDirectionPow(Vector<R> value, double exponent, double maxMagnitude)
    Raises the norm of the input to the power of the given exponent while preserving its direction.
    static double
    inputModulus(double input, double minimumInput, double maximumInput)
    Returns modulus of input.
    static double
    inverseLerp(double a, double b, double q)
    Returns the interpolant t that reflects where q is with respect to the range (a, b).
    static boolean
    isNear(double expected, double actual, double tolerance)
    Checks if the given value matches an expected value within a certain tolerance.
    static boolean
    isNear(double expected, double actual, double tolerance, double min, double max)
    Checks if the given value matches an expected value within a certain tolerance.
    static double
    lerp(double a, double b, double t)
    Computes the linear interpolation between a and b if t ∈ [0, 1) and the linear extrapolation otherwise.
    static Translation2d
    slewRateLimit(Translation2d current, Translation2d next, double dt, double maxVelocity)
    Limits translation velocity.
    static Translation3d
    slewRateLimit(Translation3d current, Translation3d next, double dt, double maxVelocity)
    Limits translation velocity.

    Methods inherited from class java.lang.Object

    clone, equals, finalize, getClass, hashCode, notify, notifyAll, toString, wait, wait, wait

  • Method Details

    • lerp

      public static double lerp(double a, double b, double t)
      Computes the linear interpolation between a and b if t ∈ [0, 1) and the linear extrapolation otherwise.
      Parameters:
      a - The start value.
      b - The end value.
      t - The fraction for interpolation.
      Returns:
      The interpolated value.

    • inverseLerp

      public static double inverseLerp(double a, double b, double q)
      Returns the interpolant t that reflects where q is with respect to the range (a, b). In other words, returns t such that q = a + (b - a)t. If a = b, then returns 0.
      Parameters:
      a - Lower part of interpolation range.
      b - Upper part of interpolation range.
      q - Query.
      Returns:
      Interpolant.

    • applyDeadband

      public static double applyDeadband(double value, double deadband, double maxMagnitude)
      Returns 0.0 if the given value is within the specified range around zero. The remaining range between the deadband and the maximum magnitude is scaled from 0.0 to the maximum magnitude.
      Parameters:
      value - Value to clip.
      deadband - Range around zero.
      maxMagnitude - The maximum magnitude of the input. Can be infinite.
      Returns:
      The value after the deadband is applied.

    • applyDeadband

      public static double applyDeadband(double value, double deadband)
      Returns 0.0 if the given value is within the specified range around zero. The remaining range between the deadband and 1.0 is scaled from 0.0 to 1.0.
      Parameters:
      value - Value to clip.
      deadband - Range around zero.
      Returns:
      The value after the deadband is applied.

    • applyDeadband

      public static <R extends Num> Vector<R> applyDeadband(Vector<R> value, double deadband, double maxMagnitude)
      Returns a zero vector if the given vector is within the specified distance from the origin. The remaining distance between the deadband and the maximum distance is scaled from the origin to the maximum distance.
      Type Parameters:
      R - The number of rows in the vector.
      Parameters:
      value - Value to clip.
      deadband - Distance from origin.
      maxMagnitude - The maximum distance from the origin of the input. Can be infinite.
      Returns:
      The value after the deadband is applied.

    • applyDeadband

      public static <R extends Num> Vector<R> applyDeadband(Vector<R> value, double deadband)
      Returns a zero vector if the given vector is within the specified distance from the origin. The remaining distance between the deadband and a distance of 1.0 is scaled from the origin to a distance of 1.0.
      Type Parameters:
      R - The number of rows in the vector.
      Parameters:
      value - Value to clip.
      deadband - Distance from origin.
      Returns:
      The value after the deadband is applied.

    • copyDirectionPow

      public static double copyDirectionPow(double value, double exponent, double maxMagnitude)
      Raises the input to the power of the given exponent while preserving its sign.

      The function normalizes the input value to the range [0, 1] based on the maximum magnitude, raises it to the power of the exponent, then scales the result back to the original range and copying the sign. This keeps the value in the original range and gives consistent curve behavior regardless of the input value's scale.

      This is useful for applying smoother or more aggressive control response curves (e.g. joystick input shaping).

      Parameters:
      value - The input value to transform.
      exponent - The exponent to apply (e.g. 1.0 = linear, 2.0 = squared curve). Must be positive.
      maxMagnitude - The maximum expected absolute value of input. Must be positive.
      Returns:
      The transformed value with the same sign and scaled to the input range.

    • copyDirectionPow

      public static double copyDirectionPow(double value, double exponent)
      Raises the input to the power of the given exponent while preserving its sign.

      This is useful for applying smoother or more aggressive control response curves (e.g. joystick input shaping).

      Parameters:
      value - The input value to transform.
      exponent - The exponent to apply (e.g. 1.0 = linear, 2.0 = squared curve). Must be positive.
      Returns:
      The transformed value with the same sign.

    • copyDirectionPow

      public static <R extends Num> Vector<R> copyDirectionPow(Vector<R> value, double exponent, double maxMagnitude)
      Raises the norm of the input to the power of the given exponent while preserving its direction.

      The function normalizes the norm of the input to the range [0, 1] based on the maximum distance, raises it to the power of the exponent, then scales the result back to the original range. This keeps the value in the original max distance and gives consistent curve behavior regardless of the input norm's scale.

      Type Parameters:
      R - The number of rows in the vector.
      Parameters:
      value - The input vector to transform.
      exponent - The exponent to apply (e.g. 1.0 = linear, 2.0 = squared curve). Must be positive.
      maxMagnitude - The maximum expected distance from origin of input. Must be positive.
      Returns:
      The transformed value with the same direction and norm scaled to the input range.

    • copyDirectionPow

      public static <R extends Num> Vector<R> copyDirectionPow(Vector<R> value, double exponent)
      Raises the norm of the input to the power of the given exponent while preserving its direction.
      Type Parameters:
      R - The number of rows in the vector.
      Parameters:
      value - The input vector to transform.
      exponent - The exponent to apply (e.g. 1.0 = linear, 2.0 = squared curve). Must be positive.
      Returns:
      The transformed value with the same direction.

    • inputModulus

      public static double inputModulus(double input, double minimumInput, double maximumInput)
      Returns modulus of input.
      Parameters:
      input - Input value to wrap.
      minimumInput - The minimum value expected from the input.
      maximumInput - The maximum value expected from the input.
      Returns:
      The wrapped value.

    • angleModulus

      public static double angleModulus(double angleRadians)
      Wraps an angle to the range -π to π radians.
      Parameters:
      angleRadians - Angle to wrap in radians.
      Returns:
      The wrapped angle.

    • isNear

      public static boolean isNear(double expected, double actual, double tolerance)
      Checks if the given value matches an expected value within a certain tolerance.
      Parameters:
      expected - The expected value
      actual - The actual value
      tolerance - The allowed difference between the actual and the expected value
      Returns:
      Whether or not the actual value is within the allowed tolerance

    • isNear

      public static boolean isNear(double expected, double actual, double tolerance, double min, double max)
      Checks if the given value matches an expected value within a certain tolerance. Supports continuous input for cases like absolute encoders.

      Continuous input means that the min and max value are considered to be the same point, and tolerances can be checked across them. A common example would be for absolute encoders: calling isNear(2, 359, 5, 0, 360) returns true because 359 is 1 away from 360 (which is treated as the same as 0) and 2 is 2 away from 0, adding up to an error of 3 degrees, which is within the given tolerance of 5.

      Parameters:
      expected - The expected value
      actual - The actual value
      tolerance - The allowed difference between the actual and the expected value
      min - Smallest value before wrapping around to the largest value
      max - Largest value before wrapping around to the smallest value
      Returns:
      Whether or not the actual value is within the allowed tolerance

    • slewRateLimit

      public static Translation2d slewRateLimit(Translation2d current, Translation2d next, double dt, double maxVelocity)
      Limits translation velocity.
      Parameters:
      current - Translation at current timestep.
      next - Translation at next timestep.
      dt - Timestep duration.
      maxVelocity - Maximum translation velocity.
      Returns:
      Returns the next Translation2d limited to maxVelocity

    • slewRateLimit

      public static Translation3d slewRateLimit(Translation3d current, Translation3d next, double dt, double maxVelocity)
      Limits translation velocity.
      Parameters:
      current - Translation at current timestep.
      next - Translation at next timestep.
      dt - Timestep duration.
      maxVelocity - Maximum translation velocity.
      Returns:
      Returns the next Translation3d limited to maxVelocity