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
007public final class MathUtil {
008  private MathUtil() {
009    throw new AssertionError("utility class");
010  }
011
012  /**
013   * Returns value clamped between low and high boundaries.
014   *
015   * @param value Value to clamp.
016   * @param low The lower boundary to which to clamp value.
017   * @param high The higher boundary to which to clamp value.
018   * @return The clamped value.
019   */
020  public static int clamp(int value, int low, int high) {
021    return Math.max(low, Math.min(value, high));
022  }
023
024  /**
025   * Returns value clamped between low and high boundaries.
026   *
027   * @param value Value to clamp.
028   * @param low The lower boundary to which to clamp value.
029   * @param high The higher boundary to which to clamp value.
030   * @return The clamped value.
031   */
032  public static double clamp(double value, double low, double high) {
033    return Math.max(low, Math.min(value, high));
034  }
035
036  /**
037   * Returns 0.0 if the given value is within the specified range around zero. The remaining range
038   * between the deadband and 1.0 is scaled from 0.0 to 1.0.
039   *
040   * @param value Value to clip.
041   * @param deadband Range around zero.
042   * @return The value after the deadband is applied.
043   */
044  public static double applyDeadband(double value, double deadband) {
045    if (Math.abs(value) > deadband) {
046      if (value > 0.0) {
047        return (value - deadband) / (1.0 - deadband);
048      } else {
049        return (value + deadband) / (1.0 - deadband);
050      }
051    } else {
052      return 0.0;
053    }
054  }
055
056  /**
057   * Returns modulus of input.
058   *
059   * @param input Input value to wrap.
060   * @param minimumInput The minimum value expected from the input.
061   * @param maximumInput The maximum value expected from the input.
062   * @return The wrapped value.
063   */
064  public static double inputModulus(double input, double minimumInput, double maximumInput) {
065    double modulus = maximumInput - minimumInput;
066
067    // Wrap input if it's above the maximum input
068    int numMax = (int) ((input - minimumInput) / modulus);
069    input -= numMax * modulus;
070
071    // Wrap input if it's below the minimum input
072    int numMin = (int) ((input - maximumInput) / modulus);
073    input -= numMin * modulus;
074
075    return input;
076  }
077
078  /**
079   * Wraps an angle to the range -pi to pi radians.
080   *
081   * @param angleRadians Angle to wrap in radians.
082   * @return The wrapped angle.
083   */
084  public static double angleModulus(double angleRadians) {
085    return inputModulus(angleRadians, -Math.PI, Math.PI);
086  }
087
088  /**
089   * Perform linear interpolation between two values.
090   *
091   * @param startValue The value to start at.
092   * @param endValue The value to end at.
093   * @param t How far between the two values to interpolate. This is clamped to [0, 1].
094   * @return The interpolated value.
095   */
096  @SuppressWarnings("ParameterName")
097  public static double interpolate(double startValue, double endValue, double t) {
098    return startValue + (endValue - startValue) * MathUtil.clamp(t, 0, 1);
099  }
100}