Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-25 01:41:43 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpimath] Refactor MathUtil.interpolate() and MathUtil.inverseInterpolate() to handle extrapolation (#8232)

Browse Source
This commit is contained in:
Tyler Veness
2025-09-20 11:22:05 -07:00
committed by GitHub
parent 1ce2854a1e
commit f701132392
12 changed files with 84 additions and 75 deletions
Download Patch File Download Diff File Expand all files Collapse all files

65
wpimath/src/main/java/edu/wpi/first/math/MathUtil.java
View File

@@ -13,6 +13,39 @@ public final class MathUtil {
throw new AssertionError("utility class");
}
/**
* Computes the linear interpolation between a and b if t ∈ [0, 1) and the linear extrapolation
* otherwise.
*
* @param a The start value.
* @param b The end value.
* @param t The fraction for interpolation.
* @return The interpolated value.
*/
public static double lerp(double a, double b, double t) {
return a + (b - a) * t;
}
/**
* Returns the interpolant t that reflects where q is with respect to the range (a, b). In other
* words, returns t such that q = a + (b - a)t. If a = b, then returns 0.
*
* @param a Lower part of interpolation range.
* @param b Upper part of interpolation range.
* @param q Query.
* @return Interpolant.
*/
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);
}
}
/**
* 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.
@@ -151,38 +184,6 @@ public final class MathUtil {
return inputModulus(angleRadians, -Math.PI, Math.PI);
}
/**
* Perform linear interpolation between two values.
*
* @param startValue The value to start at.
* @param endValue The value to end at.
* @param t How far between the two values to interpolate. This is clamped to [0, 1].
* @return The interpolated value.
*/
public static double interpolate(double startValue, double endValue, double t) {
return startValue + (endValue - startValue) * Math.clamp(t, 0, 1);
}
/**
* Return where within interpolation range [0, 1] q is between startValue and endValue.
*
* @param startValue Lower part of interpolation range.
* @param endValue Upper part of interpolation range.
* @param q Query.
* @return Interpolant in range [0, 1].
*/
public static double inverseInterpolate(double startValue, double endValue, double q) {
double totalRange = endValue - startValue;
if (totalRange <= 0) {
return 0.0;
}
double queryToStart = q - startValue;
if (queryToStart <= 0) {
return 0.0;
}
return queryToStart / totalRange;
}
/**
* Checks if the given value matches an expected value within a certain tolerance.
*

4
wpimath/src/main/java/edu/wpi/first/math/geometry/Translation2d.java
View File

@@ -358,8 +358,8 @@ public class Translation2d
@Override
public Translation2d interpolate(Translation2d endValue, double t) {
return new Translation2d(
MathUtil.interpolate(this.getX(), endValue.getX(), t),
MathUtil.interpolate(this.getY(), endValue.getY(), t));
MathUtil.lerp(this.getX(), endValue.getX(), t),
MathUtil.lerp(this.getY(), endValue.getY(), t));
}
/** Translation2d protobuf for serialization. */

6
wpimath/src/main/java/edu/wpi/first/math/geometry/Translation3d.java
View File

@@ -393,9 +393,9 @@ public class Translation3d
@Override
public Translation3d interpolate(Translation3d endValue, double t) {
return new Translation3d(
MathUtil.interpolate(this.getX(), endValue.getX(), t),
MathUtil.interpolate(this.getY(), endValue.getY(), t),
MathUtil.interpolate(this.getZ(), endValue.getZ(), t));
MathUtil.lerp(this.getX(), endValue.getX(), t),
MathUtil.lerp(this.getY(), endValue.getY(), t),
MathUtil.lerp(this.getZ(), endValue.getZ(), t));
}
/** Translation3d protobuf for serialization. */

2
wpimath/src/main/java/edu/wpi/first/math/interpolation/Interpolator.java
View File

@@ -30,6 +30,6 @@ public interface Interpolator<T> {
* @return Interpolator for Double.
*/
static Interpolator<Double> forDouble() {
return MathUtil::interpolate;
return MathUtil::lerp;
}
}

2
wpimath/src/main/java/edu/wpi/first/math/interpolation/InverseInterpolator.java
View File

@@ -30,6 +30,6 @@ public interface InverseInterpolator<T> {
* @return Inverse interpolator for Double.
*/
static InverseInterpolator<Double> forDouble() {
return MathUtil::inverseInterpolate;
return MathUtil::inverseLerp;
}
}

2
wpimath/src/main/java/edu/wpi/first/math/interpolation/TimeInterpolatableBuffer.java
View File

@@ -65,7 +65,7 @@ public final class TimeInterpolatableBuffer<T> {
* @return The new TimeInterpolatableBuffer.
*/
public static TimeInterpolatableBuffer<Double> createDoubleBuffer(double historySize) {
return new TimeInterpolatableBuffer<>(MathUtil::interpolate, historySize);
return new TimeInterpolatableBuffer<>(MathUtil::lerp, historySize);
}
/**

3
wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveWheelPositions.java
View File

@@ -77,7 +77,6 @@ public class DifferentialDriveWheelPositions
public DifferentialDriveWheelPositions interpolate(
DifferentialDriveWheelPositions endValue, double t) {
return new DifferentialDriveWheelPositions(
MathUtil.interpolate(this.left, endValue.left, t),
MathUtil.interpolate(this.right, endValue.right, t));
MathUtil.lerp(this.left, endValue.left, t), MathUtil.lerp(this.right, endValue.right, t));
}
}

8
wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveWheelPositions.java
View File

@@ -96,9 +96,9 @@ public class MecanumDriveWheelPositions
@Override
public MecanumDriveWheelPositions interpolate(MecanumDriveWheelPositions endValue, double t) {
return new MecanumDriveWheelPositions(
MathUtil.interpolate(this.frontLeft, endValue.frontLeft, t),
MathUtil.interpolate(this.frontRight, endValue.frontRight, t),
MathUtil.interpolate(this.rearLeft, endValue.rearLeft, t),
MathUtil.interpolate(this.rearRight, endValue.rearRight, t));
MathUtil.lerp(this.frontLeft, endValue.frontLeft, t),
MathUtil.lerp(this.frontRight, endValue.frontRight, t),
MathUtil.lerp(this.rearLeft, endValue.rearLeft, t),
MathUtil.lerp(this.rearRight, endValue.rearRight, t));
}
}

2
wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveModulePosition.java
View File

@@ -99,7 +99,7 @@ public class SwerveModulePosition
@Override
public SwerveModulePosition interpolate(SwerveModulePosition endValue, double t) {
return new SwerveModulePosition(
MathUtil.interpolate(this.distance, endValue.distance, t),
MathUtil.lerp(this.distance, endValue.distance, t),
this.angle.interpolate(endValue.angle, t));
}
}

17
wpimath/src/main/java/edu/wpi/first/math/trajectory/Trajectory.java
View File

@@ -5,6 +5,7 @@
package edu.wpi.first.math.trajectory;
import com.fasterxml.jackson.annotation.JsonProperty;
import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Transform2d;
import edu.wpi.first.math.trajectory.proto.TrajectoryProto;
@@ -48,18 +49,6 @@ public class Trajectory implements ProtobufSerializable {
m_totalTime = m_states.get(m_states.size() - 1).time;
}
/**
* Linearly interpolates between two values.
*
* @param startValue The start value.
* @param endValue The end value.
* @param t The fraction for interpolation.
* @return The interpolated value.
*/
private static double lerp(double startValue, double endValue, double t) {
return startValue + (endValue - startValue) * t;
}
/**
* Linearly interpolates between two poses.
*
@@ -317,7 +306,7 @@ public class Trajectory implements ProtobufSerializable {
*/
State interpolate(State endValue, double i) {
// Find the new t value.
final double newT = lerp(time, endValue.time, i);
final double newT = MathUtil.lerp(time, endValue.time, i);
// Find the delta time between the current state and the interpolated state.
final double deltaT = newT - time;
@@ -351,7 +340,7 @@ public class Trajectory implements ProtobufSerializable {
newV,
acceleration,
lerp(pose, endValue.pose, interpolationFrac),
lerp(curvature, endValue.curvature, interpolationFrac));
MathUtil.lerp(curvature, endValue.curvature, interpolationFrac));
}
@Override