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 the maximum magnitude is scaled from 0.0 to the maximum magnitude.
039   *
040   * @param value Value to clip.
041   * @param deadband Range around zero.
042   * @param maxMagnitude The maximum magnitude of the input. Can be infinite.
043   * @return The value after the deadband is applied.
044   */
045  public static double applyDeadband(double value, double deadband, double maxMagnitude) {
046    if (Math.abs(value) > deadband) {
047      if (maxMagnitude / deadband > 1.0e12) {
048        // If max magnitude is sufficiently large, the implementation encounters
049        // roundoff error.  Implementing the limiting behavior directly avoids
050        // the problem.
051        return value > 0.0 ? value - deadband : value + deadband;
052      }
053      if (value > 0.0) {
054        // Map deadband to 0 and map max to max.
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        // x₁ = deadband
062        // y₁ = 0
063        // x₂ = max
064        // y₂ = max
065        //
066        // y = (max - 0)/(max - deadband) (x - deadband) + 0
067        // y = max/(max - deadband) (x - deadband)
068        // y = max (x - deadband)/(max - deadband)
069        return maxMagnitude * (value - deadband) / (maxMagnitude - deadband);
070      } else {
071        // Map -deadband to 0 and map -max to -max.
072        //
073        // y - y₁ = m(x - x₁)
074        // y - y₁ = (y₂ - y₁)/(x₂ - x₁) (x - x₁)
075        // y = (y₂ - y₁)/(x₂ - x₁) (x - x₁) + y₁
076        //
077        // (x₁, y₁) = (-deadband, 0) and (x₂, y₂) = (-max, -max).
078        // x₁ = -deadband
079        // y₁ = 0
080        // x₂ = -max
081        // y₂ = -max
082        //
083        // y = (-max - 0)/(-max + deadband) (x + deadband) + 0
084        // y = max/(max - deadband) (x + deadband)
085        // y = max (x + deadband)/(max - deadband)
086        return maxMagnitude * (value + deadband) / (maxMagnitude - deadband);
087      }
088    } else {
089      return 0.0;
090    }
091  }
092
093  /**
094   * Returns 0.0 if the given value is within the specified range around zero. The remaining range
095   * between the deadband and 1.0 is scaled from 0.0 to 1.0.
096   *
097   * @param value Value to clip.
098   * @param deadband Range around zero.
099   * @return The value after the deadband is applied.
100   */
101  public static double applyDeadband(double value, double deadband) {
102    return applyDeadband(value, deadband, 1);
103  }
104
105  /**
106   * Returns modulus of input.
107   *
108   * @param input Input value to wrap.
109   * @param minimumInput The minimum value expected from the input.
110   * @param maximumInput The maximum value expected from the input.
111   * @return The wrapped value.
112   */
113  public static double inputModulus(double input, double minimumInput, double maximumInput) {
114    double modulus = maximumInput - minimumInput;
115
116    // Wrap input if it's above the maximum input
117    int numMax = (int) ((input - minimumInput) / modulus);
118    input -= numMax * modulus;
119
120    // Wrap input if it's below the minimum input
121    int numMin = (int) ((input - maximumInput) / modulus);
122    input -= numMin * modulus;
123
124    return input;
125  }
126
127  /**
128   * Wraps an angle to the range -pi to pi radians.
129   *
130   * @param angleRadians Angle to wrap in radians.
131   * @return The wrapped angle.
132   */
133  public static double angleModulus(double angleRadians) {
134    return inputModulus(angleRadians, -Math.PI, Math.PI);
135  }
136
137  /**
138   * Perform linear interpolation between two values.
139   *
140   * @param startValue The value to start at.
141   * @param endValue The value to end at.
142   * @param t How far between the two values to interpolate. This is clamped to [0, 1].
143   * @return The interpolated value.
144   */
145  public static double interpolate(double startValue, double endValue, double t) {
146    return startValue + (endValue - startValue) * MathUtil.clamp(t, 0, 1);
147  }
148}