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 -pi to pi 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 double clamp​(double value, double low, double high)
    Returns value clamped between low and high boundaries.
    static int clamp​(int value, int low, int high)
    Returns value clamped between low and high boundaries.
    static double inputModulus​(double input, double minimumInput, double maximumInput)
    Returns modulus of input.
    static double interpolate​(double startValue, double endValue, double t)
    Perform linear interpolation between two values.
    static double inverseInterpolate​(double startValue, double endValue, double q)
    Return where within interpolation range [0, 1] q is between startValue and endValue.
    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.

    Methods inherited from class java.lang.Object

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

  • Method Details

    • clamp

      public static int clamp​(int value, int low, int high)
      Returns value clamped between low and high boundaries.
      Parameters:
      value - Value to clamp.
      low - The lower boundary to which to clamp value.
      high - The higher boundary to which to clamp value.
      Returns:
      The clamped value.

    • clamp

      public static double clamp​(double value, double low, double high)
      Returns value clamped between low and high boundaries.
      Parameters:
      value - Value to clamp.
      low - The lower boundary to which to clamp value.
      high - The higher boundary to which to clamp value.
      Returns:
      The clamped value.

    • 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.

    • 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 -pi to pi radians.
      Parameters:
      angleRadians - Angle to wrap in radians.
      Returns:
      The wrapped angle.

    • interpolate

      public static double interpolate​(double startValue, double endValue, double t)
      Perform linear interpolation between two values.
      Parameters:
      startValue - The value to start at.
      endValue - The value to end at.
      t - How far between the two values to interpolate. This is clamped to [0, 1].
      Returns:
      The interpolated value.

    • inverseInterpolate

      public static double inverseInterpolate​(double startValue, double endValue, double q)
      Return where within interpolation range [0, 1] q is between startValue and endValue.
      Parameters:
      startValue - Lower part of interpolation range.
      endValue - Upper part of interpolation range.
      q - Query.
      Returns:
      Interpolant in range [0, 1].

    • 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