Package org.chsrobotics.lib.math

Class UtilityMath

java.lang.Object
org.chsrobotics.lib.math.UtilityMath
public class UtilityMath extends Object
Various useful small math functions.

  • Field Summary

    Fields
    Modifier and Type
    Field
    Description
    static final double
    defaultAbsoluteEpsilon
    Default constant used for epsilonEqualsAbsolute().
    static final double
    defaultProportionEpsilon
    Default constant used for epsilonEqualsProportion().

  • Constructor Summary

    Constructors
    Constructor
    Description
    UtilityMath()
     

  • Method Summary

    Modifier and Type
    Method
    Description
    static double
    acot(double ratio)
    Returns the trigonometric arccotangent (inverse cotangent) of a ratio of right triangle sides.
    static double
    acot2(double x, double y)
    Two-argument arccotangent (inverse cotangent).
    static double
    acsc(double ratio)
    Returns the trigonometric arccosecant (inverse cosecant) of a ratio of right triangle sides.
    static double
    arithmeticMean(List<Double> values)
    Computes the arithmetic mean, often called the average, of a set of numbers.
    static double
    asec(double ratio)
    Returns the trigonometric arcsecant (inverse secant) of a ratio of right triangle sides.
    static double
    clamp(double maxValueAbs, double value)
    Constrains a value under a maximum absolute value.
    static double
    clamp(double ceiling, double floor, double value)
    Constrains a value between two other numbers.
    static double
    cot(double thetaRadians)
    Takes the trigonometric cotangent of an angle.
    static double
    csc(double thetaRadians)
    Takes the trigonometric cosecant of an angle.
    static boolean
    epsilonEqualsAbsolute(double valueA, double valueB)
    Returns whether two values are equal to within an epsilon (calculated by their difference and expected from floating-point arithmetic).
    static boolean
    epsilonEqualsAbsolute(double valueA, double valueB, double absoluteEpsilon)
    Returns whether two values are equal to within an epsilon (calculated by their difference and expected from floating-point arithmetic).
    static boolean
    epsilonEqualsProportion(double valueA, double valueB)
    Returns whether two values are equal to within an epsilon (calculated multiplicatively and expected from floating-point arithmetic).
    static boolean
    epsilonEqualsProportion(double valueA, double valueB, double proportionEpsilon)
    Returns whether two values are equal to within an epsilon (calculated multiplicatively and expected from floating-point arithmetic).
    static Tuple2<Double>
    fromCartesianToPolar(double x, double y)
    Converts a point in Cartesian coordinates into a point in polar coordinates.
    static Tuple2<Double>
    fromPolarToCartesian(double angle, double magnitude)
    Converts a point in polar coordinates into a point in Cartesian coordinates.
    static edu.wpi.first.math.geometry.Rotation3d
    fromRotation2d(edu.wpi.first.math.geometry.Rotation2d original)
    Converts a Rotation2d into a Rotation3d.
    static double
    geometricMean(List<Double> values)
    Computes the geometric mean of a series of numbers.
    static edu.wpi.first.math.Matrix<edu.wpi.first.math.numbers.N2,edu.wpi.first.math.numbers.N2>
    get2DRotationMatrix(double angleRadians)
    Returns a rotation matrix for 2-dimensional vectors.
    static double
    harmonicMean(List<Double> values)
    Computes the harmonic mean of a series of numbers.
    static double
    hypotenuse(double legLengthA, double legLengthB)
    Returns the solution of the Pythagorean formula for the hypotenuse of a right triangle, with the leg lengths given.
    static boolean
    inRange(double boundaryA, double boundaryB, double toCheck)
    Returns if a number is between (inclusive) the boundary numbers.
    static double
    leg(double hypotenuse, double legLengthA)
    Returns the solution of the Pythagorean formula for a leg of a right triangle, with the other leg length and the hypotenuse length given.
    static double
    normalizeAngleDegrees(double angleDegrees)
    Normalizes an angle in degrees between 0 and 360.
    static double
    normalizeAngleRadians(double angleRadians)
    Normalizes an angle in radians between 0 and 2 pi.
    static List<Double>
    normalizeSet(List<Double> inputs, double maxAbsoluteValue)
    Scales a set of doubles symmetrically to ensure that none of them exceed a maximum absolute value, while still maintaining the same ratio.
    static Tuple2<Double>
    quadraticZeros(double coeffA, double coeffB, double coeffC)
    Finds the zeros (x-intercepts) of a quadratic (second- or lower-order polynomial) function of the form y(x) = ax^2 + bx + c.
    static double
    root(double value, int order)
    Calculates a higher order root of a number.
    static List<Double>
    scaleToSum(List<Double> inputs, double desiredSum)
    Scales a set of doubles symmetrically such that they sum to a desired number, while maintaining the same ratio.
    static double
    sec(double thetaRadians)
    Takes the trigonometric secant of an angle.
    static double
    smallestAngleDegreesBetween(double angleA, double angleB)
    Returns the angle in degrees between two other angles with the smallest absolute value.
    static double
    smallestAngleRadiansBetween(double angleA, double angleB)
    Returns the angle in radians between two other angles with the smallest absolute value.

    Methods inherited from class java.lang.Object

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

  • Field Details

    • defaultProportionEpsilon

      public static final double defaultProportionEpsilon
      Default constant used for epsilonEqualsProportion().
      See Also:

    • defaultAbsoluteEpsilon

      public static final double defaultAbsoluteEpsilon
      Default constant used for epsilonEqualsAbsolute().
      See Also:

  • Constructor Details

    • UtilityMath

      public UtilityMath()

  • Method Details

    • normalizeAngleRadians

      public static double normalizeAngleRadians(double angleRadians)
      Normalizes an angle in radians between 0 and 2 pi.
      Parameters:
      angleRadians - Value in radians of the angle.
      Returns:
      The normalized value.

    • normalizeAngleDegrees

      public static double normalizeAngleDegrees(double angleDegrees)
      Normalizes an angle in degrees between 0 and 360.
      Parameters:
      angleDegrees - Value of the angle.
      Returns:
      The normalized value.

    • smallestAngleRadiansBetween

      public static double smallestAngleRadiansBetween(double angleA, double angleB)
      Returns the angle in radians between two other angles with the smallest absolute value.

      A negative number indicates a clockwise rotation from angle a to angle b.

      Parameters:
      angleA - The first angle, in radians.
      angleB - The second angle, in raidans.
      Returns:
      A signed angle in radians representing the smallest (positive is counterclockwise) angle between angles a and b.

    • smallestAngleDegreesBetween

      public static double smallestAngleDegreesBetween(double angleA, double angleB)
      Returns the angle in degrees between two other angles with the smallest absolute value.

      A negative number indicates a clockwise rotation from angle a to angle b.

      Parameters:
      angleA - The first angle, in degrees.
      angleB - The second angle, in degrees.
      Returns:
      A signed angle in degrees representing the smallest (positive is counterclockwise) angle between angles a and b.

    • scaleToSum

      public static List<Double> scaleToSum(List<Double> inputs, double desiredSum)
      Scales a set of doubles symmetrically such that they sum to a desired number, while maintaining the same ratio.
      Parameters:
      inputs - A list of doubles. If empty, this will return an empty list. If this sums to zero, this will be returned.
      desiredSum - The desired sum, positive or negative, of the outputs. If equal to zero, this will return a list of zeros of length equal to the input's length.
      Returns:
      A list of doubles with the same ratios between each other as the inputs.

    • normalizeSet

      public static List<Double> normalizeSet(List<Double> inputs, double maxAbsoluteValue)
      Scales a set of doubles symmetrically to ensure that none of them exceed a maximum absolute value, while still maintaining the same ratio.
      Parameters:
      inputs - A list of the input values. If empty, this will return an empty array.
      maxAbsoluteValue - The maximum absolute value allowed for an output.
      Returns:
      A list of the scaled values, in the same order as they were input.

    • clamp

      public static double clamp(double ceiling, double floor, double value)
      Constrains a value between two other numbers.
      Parameters:
      ceiling - The maximum allowed value of the output.
      floor - The minimum allowed value of the output.
      value - The value to constrain.
      Returns:
      The constrained value.

    • clamp

      public static double clamp(double maxValueAbs, double value)
      Constrains a value under a maximum absolute value.
      Parameters:
      maxValueAbs - The maximum allowed absolute value of the output.
      value - The value to constrain.
      Returns:
      The constrained value.

    • quadraticZeros

      public static Tuple2<Double> quadraticZeros(double coeffA, double coeffB, double coeffC)
      Finds the zeros (x-intercepts) of a quadratic (second- or lower-order polynomial) function of the form y(x) = ax^2 + bx + c.

      If there are no real-valued solutions, this will return NaN represented twice in the Tuple.

      If there is one real-valued solution, this will return that solution represented twice in the Tuple.

      Parameters:
      coeffA - The coefficient of the ax^2 term of the quadratic polynomial.
      coeffB - The coefficient of the bx term of the quadratic polynomial.
      coeffC - The coefficient of the c (constant) term of the quadratic polynomial.
      Returns:
      A Tuple2 holding up to 2 unique real solutions (if they exist).

    • epsilonEqualsAbsolute

      public static boolean epsilonEqualsAbsolute(double valueA, double valueB, double absoluteEpsilon)
      Returns whether two values are equal to within an epsilon (calculated by their difference and expected from floating-point arithmetic). Essentially just "close enough".
      Parameters:
      valueA - The first number to compare.
      valueB - The second number to compare.
      absoluteEpsilon - The maximum allowed difference between the two to return true.
      Returns:
      Whether two values are "close enough" to each other.

    • epsilonEqualsAbsolute

      public static boolean epsilonEqualsAbsolute(double valueA, double valueB)
      Returns whether two values are equal to within an epsilon (calculated by their difference and expected from floating-point arithmetic). Essentially just "close enough".

      Uses a default epsilon value of 1E-5 (0.00001).

      Parameters:
      valueA - The first number to compare.
      valueB - The second number to compare.
      Returns:
      Whether two values are "close enough" to each other.

    • epsilonEqualsProportion

      public static boolean epsilonEqualsProportion(double valueA, double valueB, double proportionEpsilon)
      Returns whether two values are equal to within an epsilon (calculated multiplicatively and expected from floating-point arithmetic). Essentially just "close enough".

      To avoid division by zero, if only one of the two terms is exactly equal to zero, this will return false. If *both* are exactly equal to zero, this returns true.

      Parameters:
      valueA - The first number to compare.
      valueB - The second number to compare.
      proportionEpsilon - If one value is bigger than another by a factor of at least 1 + proportionEpsilon, this will return false.
      Returns:
      Whether two values are "close enough" to each other.

    • epsilonEqualsProportion

      public static boolean epsilonEqualsProportion(double valueA, double valueB)
      Returns whether two values are equal to within an epsilon (calculated multiplicatively and expected from floating-point arithmetic). Essentially just "close enough".

      Uses a default epsilon value of 1E-3 (0.001).

      To avoid division by zero, if only one of the two terms is exactly equal to zero, this will return false. If *both* are exactly equal to zero, this returns true.

      Parameters:
      valueA - The first number to compare.
      valueB - The second number to compare.
      Returns:
      Whether two values are "close enough" to each other.

    • hypotenuse

      public static double hypotenuse(double legLengthA, double legLengthB)
      Returns the solution of the Pythagorean formula for the hypotenuse of a right triangle, with the leg lengths given.
      Parameters:
      legLengthA - The length of one of the legs.
      legLengthB - The length of the other leg.
      Returns:
      The length of the hypotenuse.

    • leg

      public static double leg(double hypotenuse, double legLengthA)
      Returns the solution of the Pythagorean formula for a leg of a right triangle, with the other leg length and the hypotenuse length given.
      Parameters:
      hypotenuse - The length of the hypotenuse.
      legLengthA - The length of one of the legs.
      Returns:
      The length of the hypotenuse.

    • root

      public static double root(double value, int order)
      Calculates a higher order root of a number.
      Parameters:
      value - The value to take the root of.
      order - The order of the root (2 is a square root, 3 is a cubic root, etc).
      Returns:
      The positive root. If order is equal to 0, will return NaN.

    • csc

      public static double csc(double thetaRadians)
      Takes the trigonometric cosecant of an angle.
      Parameters:
      thetaRadians - The angle. If coterminal to 0 or pi, this will return NaN.
      Returns:
      The cosecant of the angle.

    • sec

      public static double sec(double thetaRadians)
      Takes the trigonometric secant of an angle.
      Parameters:
      thetaRadians - The angle. If coterminal to pi/2 or -pi/2, this will return NaN.
      Returns:
      The secant of the angle.

    • cot

      public static double cot(double thetaRadians)
      Takes the trigonometric cotangent of an angle.
      Parameters:
      thetaRadians - The angle. If coterminal to 0 or pi, this will return NaN.
      Returns:
      The cotangent of the angle.

    • acsc

      public static double acsc(double ratio)
      Returns the trigonometric arccosecant (inverse cosecant) of a ratio of right triangle sides.
      Parameters:
      ratio - The ratio. If equal to 0, this will return NaN.
      Returns:
      The arccosecant of the ratio.

    • asec

      public static double asec(double ratio)
      Returns the trigonometric arcsecant (inverse secant) of a ratio of right triangle sides.
      Parameters:
      ratio - The ratio. If equal to 0, this will return NaN.
      Returns:
      The arcsecant of the ratio.

    • acot

      public static double acot(double ratio)
      Returns the trigonometric arccotangent (inverse cotangent) of a ratio of right triangle sides.
      Parameters:
      ratio - The ratio. If equal to 0, this will return NaN.
      Returns:
      The arccotangent of the ratio.

    • acot2

      public static double acot2(double x, double y)
      Two-argument arccotangent (inverse cotangent). This is useful over the single argument arccotangent to avoid sign erasure (allowing returned angles in quadrange III instead of just I).
      Parameters:
      x - A side length.
      y - A side length.
      Returns:
      The angle associated with the sides.

    • inRange

      public static boolean inRange(double boundaryA, double boundaryB, double toCheck)
      Returns if a number is between (inclusive) the boundary numbers. The boundaries do not have to be ordered.
      Parameters:
      boundaryA - The first boundary value.
      boundaryB - The second boundary value.
      toCheck - The value to check.
      Returns:
      Whether the value is between (inclusive) the boundary numbers in either ordering.

    • fromPolarToCartesian

      public static Tuple2<Double> fromPolarToCartesian(double angle, double magnitude)
      Converts a point in polar coordinates into a point in Cartesian coordinates.
      Parameters:
      angle - The angle, in radians counterclockwise from the positive x-axis, of the point vector.
      magnitude - The magnitude of the point vector.
      Returns:
      A Tuple2 of the Cartesian coordinates, as (x, y).

    • fromCartesianToPolar

      public static Tuple2<Double> fromCartesianToPolar(double x, double y)
      Converts a point in Cartesian coordinates into a point in polar coordinates.
      Parameters:
      x - The x-value of the point.
      y - The y-value of the point.
      Returns:
      A Tuple2 of the polar coordinates, as (angle, magnitude).

    • arithmeticMean

      public static double arithmeticMean(List<Double> values)
      Computes the arithmetic mean, often called the average, of a set of numbers.
      Parameters:
      values - The numbers to find the arithmetic mean of. If empty, this will return 0.
      Returns:
      The arithmetic mean.

    • geometricMean

      public static double geometricMean(List<Double> values)
      Computes the geometric mean of a series of numbers.
      Parameters:
      values - The numbers to find the geometric mean of. If empty, this will return 0.
      Returns:
      The geometric mean.

    • harmonicMean

      public static double harmonicMean(List<Double> values)
      Computes the harmonic mean of a series of numbers.

      If a number input to this is 0, its reciprocal will be defined as 0, and if the arithmetic mean before reciprocation is 0, this will return 0.

      Parameters:
      values - The numbers to find the harmonic mean of. If empty, this will return 0.
      Returns:
      The geometric mean.

    • fromRotation2d

      public static edu.wpi.first.math.geometry.Rotation3d fromRotation2d(edu.wpi.first.math.geometry.Rotation2d original)
      Converts a Rotation2d into a Rotation3d.
      Parameters:
      original - The Rotation2d to convert.
      Returns:
      A Rotation3d with a pitch and roll of 0, and a yaw of the Rotation2d's value.

    • get2DRotationMatrix

      public static edu.wpi.first.math.Matrix<edu.wpi.first.math.numbers.N2,edu.wpi.first.math.numbers.N2> get2DRotationMatrix(double angleRadians)
      Returns a rotation matrix for 2-dimensional vectors.

      To use, multiply the returned matrix by a column vector (WPILib Vector is a column vector). Note that matrix multiplication is non-commutative and the rotation matrix is only valid if it is the first of the multiplicands.

      Parameters:
      angleRadians - An angle, in radians, to generate a rotation matrix for.
      Returns:
      The 2-dimensional rotation matrix for the given angle.