MathUtil.java

// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
 
package org.wpilib.math.util;
 
import org.wpilib.math.geometry.Translation2d;
import org.wpilib.math.geometry.Translation3d;
import org.wpilib.math.linalg.Vector;

public final class MathUtil {
  private MathUtil() {
    throw new AssertionError("utility class");
  }

  public static double lerp(double a, double b, double t) {
    return a + (b - a) * t;
  }

  public static double inverseLerp(double a, double b, double q) {
    // q = a + (b − a)t
    // (b − a)t = q − a
    // t = (q − a)/(b − a)
    if (Math.abs(a - b) < 1e-9) {
      return 0.0;
    } else {
      return (q - a) / (b - a);
    }
  }

  public static double applyDeadband(double value, double deadband, double maxMagnitude) {
    if (Math.abs(value) < deadband) {
      return 0;
    }
    if (value > 0.0) {
      // Map deadband to 0 and map max to max with a linear relationship.
      //
      //   y - y₁ = m(x - x₁)
      //   y - y₁ = (y₂ - y₁)/(x₂ - x₁) (x - x₁)
      //   y = (y₂ - y₁)/(x₂ - x₁) (x - x₁) + y₁
      //
      // (x₁, y₁) = (deadband, 0) and (x₂, y₂) = (max, max).
      //
      //   x₁ = deadband
      //   y₁ = 0
      //   x₂ = max
      //   y₂ = max
      //   y = (max - 0)/(max - deadband) (x - deadband) + 0
      //   y = max/(max - deadband) (x - deadband)
      //
      // To handle high values of max, rewrite so that max only appears on the denominator.
      //
      //   y = ((max - deadband) + deadband)/(max - deadband) (x - deadband)
      //   y = (1 + deadband/(max - deadband)) (x - deadband)
      return (1 + deadband / (maxMagnitude - deadband)) * (value - deadband);
    } else {
      // Map -deadband to 0 and map -max to -max with a linear relationship.
      //
      //   y - y₁ = m(x - x₁)
      //   y - y₁ = (y₂ - y₁)/(x₂ - x₁) (x - x₁)
      //   y = (y₂ - y₁)/(x₂ - x₁) (x - x₁) + y₁
      //
      // (x₁, y₁) = (-deadband, 0) and (x₂, y₂) = (-max, -max).
      //
      //   x₁ = -deadband
      //   y₁ = 0
      //   x₂ = -max
      //   y₂ = -max
      //   y = (-max - 0)/(-max + deadband) (x + deadband) + 0
      //   y = max/(max - deadband) (x + deadband)
      //
      // To handle high values of max, rewrite so that max only appears on the denominator.
      //
      //   y = ((max - deadband) + deadband)/(max - deadband) (x + deadband)
      //   y = (1 + deadband/(max - deadband)) (x + deadband)
      return (1 + deadband / (maxMagnitude - deadband)) * (value + deadband);
    }
  }

  public static double applyDeadband(double value, double deadband) {
    return applyDeadband(value, deadband, 1);
  }

  public static <R extends Num> Vector<R> applyDeadband(
      Vector<R> value, double deadband, double maxMagnitude) {
    if (value.norm() < 1e-9) {
      return value.times(0);
    }
    return value.unit().times(applyDeadband(value.norm(), deadband, maxMagnitude));
  }

  public static <R extends Num> Vector<R> applyDeadband(Vector<R> value, double deadband) {
    return applyDeadband(value, deadband, 1);
  }

  public static double copyDirectionPow(double value, double exponent, double maxMagnitude) {
    return Math.copySign(Math.pow(Math.abs(value) / maxMagnitude, exponent), value) * maxMagnitude;
  }

  public static double copyDirectionPow(double value, double exponent) {
    return copyDirectionPow(value, exponent, 1);
  }

  public static <R extends Num> Vector<R> copyDirectionPow(
      Vector<R> value, double exponent, double maxMagnitude) {
    if (value.norm() < 1e-9) {
      return value.times(0);
    }
    return value.unit().times(copyDirectionPow(value.norm(), exponent, maxMagnitude));
  }

  public static <R extends Num> Vector<R> copyDirectionPow(Vector<R> value, double exponent) {
    return copyDirectionPow(value, exponent, 1);
  }

  public static double inputModulus(double input, double minimumInput, double maximumInput) {
    double modulus = maximumInput - minimumInput;
 
    // Wrap input if it's above the maximum input
    int numMax = (int) ((input - minimumInput) / modulus);
    input -= numMax * modulus;
 
    // Wrap input if it's below the minimum input
    int numMin = (int) ((input - maximumInput) / modulus);
    input -= numMin * modulus;
 
    return input;
  }

  public static double angleModulus(double angleRadians) {
    return inputModulus(angleRadians, -Math.PI, Math.PI);
  }

  public static boolean isNear(double expected, double actual, double tolerance) {
    if (tolerance < 0) {
      throw new IllegalArgumentException("Tolerance must be a non-negative number!");
    }
    return Math.abs(expected - actual) < tolerance;
  }

  public static boolean isNear(
      double expected, double actual, double tolerance, double min, double max) {
    if (tolerance < 0) {
      throw new IllegalArgumentException("Tolerance must be a non-negative number!");
    }
    // Max error is exactly halfway between the min and max
    double errorBound = (max - min) / 2.0;
    double error = MathUtil.inputModulus(expected - actual, -errorBound, errorBound);
    return Math.abs(error) < tolerance;
  }

  public static Translation2d slewRateLimit(
      Translation2d current, Translation2d next, double dt, double maxVelocity) {
    if (maxVelocity < 0) {
      Exception e = new IllegalArgumentException();
      MathSharedStore.reportError(
          "maxVelocity must be a non-negative number, got " + maxVelocity, e.getStackTrace());
      return next;
    }
    Translation2d diff = next.minus(current);
    double dist = diff.getNorm();
    if (dist < 1e-9) {
      return next;
    }
    if (dist > maxVelocity * dt) {
      // Move maximum allowed amount in direction of the difference
      return current.plus(diff.times(maxVelocity * dt / dist));
    }
    return next;
  }

  public static Translation3d slewRateLimit(
      Translation3d current, Translation3d next, double dt, double maxVelocity) {
    if (maxVelocity < 0) {
      Exception e = new IllegalArgumentException();
      MathSharedStore.reportError(
          "maxVelocity must be a non-negative number, got " + maxVelocity, e.getStackTrace());
      return next;
    }
    Translation3d diff = next.minus(current);
    double dist = diff.getNorm();
    if (dist < 1e-9) {
      return next;
    }
    if (dist > maxVelocity * dt) {
      // Move maximum allowed amount in direction of the difference
      return current.plus(diff.times(maxVelocity * dt / dist));
    }
    return next;
  }
}