Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-29 02:21:44 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpimath] Fix Rotation2d interpolation in Java (#4125)

Browse Source 
Fixes #4112.
This commit is contained in:
Tyler Veness
2022-03-29 08:42:43 -07:00
committed by GitHub
parent 2e462a19d3
commit b4620f01f9
2 changed files with 16 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation2d.java
View File

@@ -206,6 +206,6 @@ public class Rotation2d implements Interpolatable<Rotation2d> {
@Override
@SuppressWarnings("ParameterName")
public Rotation2d interpolate(Rotation2d endValue, double t) {
return new Rotation2d(MathUtil.interpolate(this.getRadians(), endValue.getRadians(), t));
return plus(endValue.minus(this).times(MathUtil.clamp(t, 0, 1)));
}
}

15
wpimath/src/test/java/edu/wpi/first/math/geometry/Rotation2dTest.java
View File

@@ -76,4 +76,19 @@ class Rotation2dTest {
var rot2 = Rotation2d.fromDegrees(43.5);
assertNotEquals(rot1, rot2);
}
@Test
void testInterpolate() {
// 50 + (70 - 50) * 0.5 = 60
var rot1 = Rotation2d.fromDegrees(50);
var rot2 = Rotation2d.fromDegrees(70);
var interpolated = rot1.interpolate(rot2, 0.5);
assertEquals(60.0, interpolated.getDegrees(), 1e-2);
// -160 minus half distance between 170 and -160 (15) = -175
var rot3 = Rotation2d.fromDegrees(170);
var rot4 = Rotation2d.fromDegrees(-160);
interpolated = rot3.interpolate(rot4, 0.5);
assertEquals(-175.0, interpolated.getDegrees());
}
}