From b4620f01f92ad766c2a83702384df8519e4eb20c Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Tue, 29 Mar 2022 08:42:43 -0700 Subject: [PATCH] [wpimath] Fix Rotation2d interpolation in Java (#4125) Fixes #4112. --- .../edu/wpi/first/math/geometry/Rotation2d.java | 2 +- .../wpi/first/math/geometry/Rotation2dTest.java | 15 +++++++++++++++ 2 files changed, 16 insertions(+), 1 deletion(-) diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation2d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation2d.java index 838a8310c7..25f7b93985 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation2d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation2d.java @@ -206,6 +206,6 @@ public class Rotation2d implements Interpolatable { @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))); } } diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/Rotation2dTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/Rotation2dTest.java index cb3f0f3dfd..87055ef210 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/geometry/Rotation2dTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/Rotation2dTest.java @@ -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()); + } }