[wpimath] Revert Rotation2D change that limits angles (#4781)

Reverts "[wpimath] Constrain Rotation2d range to -pi to pi (#4611)"
This reverts commit d1d458db2b.

This broke multiple teams code in beta. It is also easier to limit the angle externally, then reconstruct a larger angle that got limited. This additionally adds comments to clarify the behavior and retains tests that were added in the reverted commit, and fixes a javadoc comment angle reference.

wpilibc/src/main/native/include/frc/interfaces/Gyro.h

@@ -66,6 +66,10 @@ class Gyro {
   /**
    * Return the heading of the robot as a Rotation2d.
    *
+   * The angle is continuous, that is it will continue from 360 to 361 degrees.
+   * This allows algorithms that wouldn't want to see a discontinuity in the
+   * gyro output as it sweeps past from 360 to 0 on the second time around.
+   *
    * The angle is expected to increase as the gyro turns counterclockwise when
    * looked at from the top. It needs to follow the NWU axis convention.
    *

wpilibj/src/main/java/edu/wpi/first/wpilibj/interfaces/Gyro.java

@@ -52,6 +52,10 @@ public interface Gyro extends AutoCloseable {
   /**
    * Return the heading of the robot as a {@link edu.wpi.first.math.geometry.Rotation2d}.
    *
+   * <p>The angle is continuous, that is it will continue from 360 to 361 degrees. This allows
+   * algorithms that wouldn't want to see a discontinuity in the gyro output as it sweeps past from
+   * 360 to 0 on the second time around.
+   *
    * <p>The angle is expected to increase as the gyro turns counterclockwise when looked at from the
    * top. It needs to follow the NWU axis convention.
    *

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

@@ -15,15 +15,21 @@ import java.util.Objects;
 
 /**
  * A rotation in a 2D coordinate frame represented by a point on the unit circle (cosine and sine).
+ *
+ * <p>The angle is continuous, that is if a Rotation2d is constructed with 361 degrees, it will
+ * return 361 degrees. This allows algorithms that wouldn't want to see a discontinuity in the
+ * rotations as it sweeps past from 360 to 0 on the second time around.
  */
 @JsonIgnoreProperties(ignoreUnknown = true)
 @JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE)
 public class Rotation2d implements Interpolatable<Rotation2d> {
+  private final double m_value;
   private final double m_cos;
   private final double m_sin;
 
   /** Constructs a Rotation2d with a default angle of 0 degrees. */
   public Rotation2d() {
+    m_value = 0.0;
     m_cos = 1.0;
     m_sin = 0.0;
   }
@@ -35,7 +41,9 @@ public class Rotation2d implements Interpolatable<Rotation2d> {
    */
   @JsonCreator
   public Rotation2d(@JsonProperty(required = true, value = "radians") double value) {
-    this(Math.cos(value), Math.sin(value));
+    m_value = value;
+    m_cos = Math.cos(value);
+    m_sin = Math.sin(value);
   }
 
   /**
@@ -53,12 +61,13 @@ public class Rotation2d implements Interpolatable<Rotation2d> {
       m_sin = 0.0;
       m_cos = 1.0;
     }
+    m_value = Math.atan2(m_sin, m_cos);
   }
 
   /**
    * Constructs and returns a Rotation2d with the given radian value.
    *
-   * @param radians The value of the angle in degrees.
+   * @param radians The value of the angle in radians.
    * @return The rotation object with the desired angle value.
    */
   public static Rotation2d fromRadians(double radians) {
@@ -118,7 +127,7 @@ public class Rotation2d implements Interpolatable<Rotation2d> {
    * @return The inverse of the current rotation.
    */
   public Rotation2d unaryMinus() {
-    return new Rotation2d(m_cos, -m_sin);
+    return new Rotation2d(-m_value);
   }
 
   /**
@@ -128,7 +137,7 @@ public class Rotation2d implements Interpolatable<Rotation2d> {
    * @return The new scaled Rotation2d.
    */
   public Rotation2d times(double scalar) {
-    return new Rotation2d(getRadians() * scalar);
+    return new Rotation2d(m_value * scalar);
   }
 
   /**
@@ -161,22 +170,22 @@ public class Rotation2d implements Interpolatable<Rotation2d> {
   }
 
   /**
-   * Returns the radian value of the Rotation2d within (-pi, pi].
+   * Returns the radian value of the Rotation2d.
    *
    * @return The radian value of the Rotation2d.
    */
   @JsonProperty
   public double getRadians() {
-    return Math.atan2(m_sin, m_cos);
+    return m_value;
   }
 
   /**
-   * Returns the degree value of the Rotation2d within (-180, 180].
+   * Returns the degree value of the Rotation2d.
    *
    * @return The degree value of the Rotation2d.
    */
   public double getDegrees() {
-    return Math.toDegrees(getRadians());
+    return Math.toDegrees(m_value);
   }
 
   /**
@@ -185,7 +194,7 @@ public class Rotation2d implements Interpolatable<Rotation2d> {
    * @return The number of rotations of the Rotation2d.
    */
   public double getRotations() {
-    return Units.radiansToRotations(getRadians());
+    return Units.radiansToRotations(m_value);
   }
 
   /**
@@ -217,7 +226,7 @@ public class Rotation2d implements Interpolatable<Rotation2d> {
 
   @Override
   public String toString() {
-    return String.format("Rotation2d(Rads: %.2f, Deg: %.2f)", getRadians(), getDegrees());
+    return String.format("Rotation2d(Rads: %.2f, Deg: %.2f)", m_value, Math.toDegrees(m_value));
   }
 
   /**
@@ -237,7 +246,7 @@ public class Rotation2d implements Interpolatable<Rotation2d> {
 
   @Override
   public int hashCode() {
-    return Objects.hash(getRadians());
+    return Objects.hash(m_value);
   }
 
   @Override

wpimath/src/main/native/include/frc/geometry/Rotation2d.h

@@ -17,6 +17,11 @@ namespace frc {
 /**
  * A rotation in a 2D coordinate frame represented by a point on the unit circle
  * (cosine and sine).
+ *
+ * The angle is continuous, that is if a Rotation2d is constructed with 361
+ * degrees, it will return 361 degrees. This allows algorithms that wouldn't
+ * want to see a discontinuity in the rotations as it sweeps past from 360 to 0
+ * on the second time around.
  */
 class WPILIB_DLLEXPORT Rotation2d {
  public:
@@ -124,18 +129,18 @@ class WPILIB_DLLEXPORT Rotation2d {
   constexpr Rotation2d RotateBy(const Rotation2d& other) const;
 
   /**
-   * Returns the radian value of the rotation within (-pi, pi].
+   * Returns the radian value of the rotation.
    *
    * @return The radian value of the rotation.
    */
-  constexpr units::radian_t Radians() const;
+  constexpr units::radian_t Radians() const { return m_value; }
 
   /**
-   * Returns the degree value of the rotation within (-180, 180].
+   * Returns the degree value of the rotation.
    *
    * @return The degree value of the rotation.
    */
-  constexpr units::degree_t Degrees() const;
+  constexpr units::degree_t Degrees() const { return m_value; }
 
   /**
    * Returns the cosine of the rotation.
@@ -159,6 +164,7 @@ class WPILIB_DLLEXPORT Rotation2d {
   constexpr double Tan() const { return Sin() / Cos(); }
 
  private:
+  units::radian_t m_value = 0_rad;
   double m_cos = 1;
   double m_sin = 0;
 };

wpimath/src/main/native/include/frc/geometry/Rotation2d.inc

@@ -13,10 +13,11 @@
 namespace frc {
 
 constexpr Rotation2d::Rotation2d(units::radian_t value)
-    : Rotation2d(std::is_constant_evaluated() ? gcem::cos(value.to<double>())
-                                              : std::cos(value.to<double>()),
-                 std::is_constant_evaluated() ? gcem::sin(value.to<double>())
-                                              : std::sin(value.to<double>())) {}
+    : m_value(value),
+      m_cos(std::is_constant_evaluated() ? gcem::cos(value.to<double>())
+                                         : std::cos(value.to<double>())),
+      m_sin(std::is_constant_evaluated() ? gcem::sin(value.to<double>())
+                                         : std::sin(value.to<double>())) {}
 
 constexpr Rotation2d::Rotation2d(units::degree_t value)
     : Rotation2d(units::radian_t{value}) {}
@@ -30,14 +31,17 @@ constexpr Rotation2d::Rotation2d(double x, double y) {
     m_sin = 0.0;
     m_cos = 1.0;
   }
+  m_value =
+      units::radian_t{std::is_constant_evaluated() ? gcem::atan2(m_sin, m_cos)
+                                                   : std::atan2(m_sin, m_cos)};
 }
 
 constexpr Rotation2d Rotation2d::operator-() const {
-  return Rotation2d{m_cos, -m_sin};
+  return Rotation2d{-m_value};
 }
 
 constexpr Rotation2d Rotation2d::operator*(double scalar) const {
-  return Rotation2d(Radians() * scalar);
+  return Rotation2d(m_value * scalar);
 }
 
 constexpr Rotation2d Rotation2d::operator+(const Rotation2d& other) const {
@@ -63,14 +67,4 @@ constexpr Rotation2d Rotation2d::RotateBy(const Rotation2d& other) const {
           Cos() * other.Sin() + Sin() * other.Cos()};
 }
 
-constexpr units::radian_t Rotation2d::Radians() const {
-  return units::radian_t{std::is_constant_evaluated()
-                             ? gcem::atan2(m_sin, m_cos)
-                             : std::atan2(m_sin, m_cos)};
-}
-
-constexpr units::degree_t Rotation2d::Degrees() const {
-  return Radians();
-}
-
 }  // namespace frc

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

@@ -13,15 +13,6 @@ import org.junit.jupiter.api.Test;
 class Rotation2dTest {
   private static final double kEpsilon = 1E-9;
 
-  @Test
-  void testInScope() {
-    var rot1 = Rotation2d.fromRadians(Math.PI * 5 / 2);
-    var rot2 = Rotation2d.fromRadians(Math.PI * 7 / 2);
-
-    assertEquals(Math.PI / 2, rot1.getRadians(), kEpsilon);
-    assertEquals(-Math.PI / 2, rot2.getRadians(), kEpsilon);
-  }
-
   @Test
   void testRadiansToDegrees() {
     var rot1 = Rotation2d.fromRadians(Math.PI / 3);
@@ -80,7 +71,7 @@ class Rotation2dTest {
     var rot = Rotation2d.fromDegrees(10.0);
 
     assertEquals(30.0, rot.times(3.0).getDegrees(), kEpsilon);
-    assertEquals(50.0, rot.times(41.0).getDegrees(), kEpsilon);
+    assertEquals(410.0, rot.times(41.0).getDegrees(), kEpsilon);
   }
 
   @Test

wpimath/src/test/native/cpp/geometry/Rotation2dTest.cpp

@@ -10,16 +10,6 @@
 
 using namespace frc;
 
-TEST(Rotation2dTest, InScope) {
-  const auto rot1 = Rotation2d{units::radian_t{std::numbers::pi * 5 / 2}};
-  const auto rot2 = Rotation2d{units::radian_t{std::numbers::pi * 7 / 2}};
-  const auto rot3 = Rotation2d{270_deg};
-
-  EXPECT_DOUBLE_EQ(std::numbers::pi / 2, rot1.Radians().value());
-  EXPECT_DOUBLE_EQ(-std::numbers::pi / 2, rot2.Radians().value());
-  EXPECT_DOUBLE_EQ(-90, rot3.Degrees().value());
-}
-
 TEST(Rotation2dTest, RadiansToDegrees) {
   const Rotation2d rot1{units::radian_t{std::numbers::pi / 3.0}};
   const Rotation2d rot2{units::radian_t{std::numbers::pi / 4.0}};
@@ -76,8 +66,8 @@ TEST(Rotation2dTest, Inequality) {
 
 TEST(Rotation2dTest, Constexpr) {
   constexpr Rotation2d defaultCtor;
-  constexpr Rotation2d radianCtor{0.25_rad};
-  constexpr Rotation2d degreeCtor{-90_deg};
+  constexpr Rotation2d radianCtor{5_rad};
+  constexpr Rotation2d degreeCtor{270_deg};
   constexpr Rotation2d rotation45{45_deg};
   constexpr Rotation2d cartesianCtor{3.5, -3.5};
 
@@ -86,9 +76,9 @@ TEST(Rotation2dTest, Constexpr) {
   constexpr auto subtracted = cartesianCtor - degreeCtor;
 
   static_assert(defaultCtor.Radians() == 0_rad);
-  static_assert(degreeCtor.Degrees() == -90_deg);
-  static_assert(negated.Radians() == -0.25_rad);
-  static_assert(multiplied.Radians() == 0.5_rad);
+  static_assert(degreeCtor.Degrees() == 270_deg);
+  static_assert(negated.Radians() == -5_rad);
+  static_assert(multiplied.Radians() == 10_rad);
   static_assert(subtracted == rotation45);
   static_assert(radianCtor != degreeCtor);
 }