wpilibc: Add unit-safety to C++ geometry classes (#1811)

wpilibc/src/main/native/include/frc/geometry/Pose2d.h

@@ -40,7 +40,7 @@ class Pose2d {
    * @param y The y component of the translational component of the pose.
    * @param rotation The rotational component of the pose.
    */
-  Pose2d(double x, double y, Rotation2d rotation);
+  Pose2d(units::meter_t x, units::meter_t y, Rotation2d rotation);
 
   /**
    * Transforms the pose by the given transformation and returns the new

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

@@ -7,6 +7,7 @@
 
 #pragma once
 
+#include <units/units.h>
 #include <wpi/math>
 
 namespace frc {
@@ -27,7 +28,7 @@ class Rotation2d {
    *
    * @param value The value of the angle in radians.
    */
-  explicit Rotation2d(double value);
+  explicit Rotation2d(units::radian_t value);
 
   /**
    * Constructs a Rotation2d with the given x and y (cosine and sine)
@@ -38,15 +39,6 @@ class Rotation2d {
    */
   Rotation2d(double x, double y);
 
-  /**
-   * Constructs and returns a Rotation2d with the given degree value.
-   *
-   * @param degrees The value of the angle in degrees.
-   *
-   * @return The rotation object with the desired angle value.
-   */
-  static Rotation2d FromDegrees(double degrees);
-
   /**
    * Adds two rotations together, with the result being bounded between -pi and
    * pi.
@@ -124,14 +116,14 @@ class Rotation2d {
    *
    * @return The radian value of the rotation.
    */
-  double Radians() const { return m_value; }
+  units::radian_t Radians() const { return m_value; }
 
   /**
    * Returns the degree value of the rotation.
    *
    * @return The degree value of the rotation.
    */
-  double Degrees() const { return Rad2Deg(m_value); }
+  units::degree_t Degrees() const { return m_value; }
 
   /**
    * Returns the cosine of the rotation.
@@ -155,18 +147,8 @@ class Rotation2d {
   double Tan() const { return m_sin / m_cos; }
 
  private:
-  double m_value = 0;
+  units::radian_t m_value = 0_deg;
   double m_cos = 1;
   double m_sin = 0;
-
-  template <typename T>
-  static T Rad2Deg(const T& rad) {
-    return rad * 180.0 / wpi::math::pi;
-  }
-
-  template <typename T>
-  static T Deg2Rad(const T& deg) {
-    return deg * wpi::math::pi / 180.0;
-  }
 };
 }  // namespace frc

wpilibc/src/main/native/include/frc/geometry/Translation2d.h

@@ -7,6 +7,8 @@
 
 #pragma once
 
+#include <units/units.h>
+
 #include "Rotation2d.h"
 
 namespace frc {
@@ -33,7 +35,7 @@ class Translation2d {
    * @param x The x component of the translation.
    * @param y The y component of the translation.
    */
-  Translation2d(double x, double y);
+  Translation2d(units::meter_t x, units::meter_t y);
 
   /**
    * Calculates the distance between two translations in 2d space.
@@ -45,28 +47,28 @@ class Translation2d {
    *
    * @return The distance between the two translations.
    */
-  double Distance(const Translation2d& other) const;
+  units::meter_t Distance(const Translation2d& other) const;
 
   /**
    * Returns the X component of the translation.
    *
    * @return The x component of the translation.
    */
-  double X() const { return m_x; }
+  units::meter_t X() const { return m_x; }
 
   /**
    * Returns the Y component of the translation.
    *
    * @return The y component of the translation.
    */
-  double Y() const { return m_y; }
+  units::meter_t Y() const { return m_y; }
 
   /**
    * Returns the norm, or distance from the origin to the translation.
    *
    * @return The norm of the translation.
    */
-  double Norm() const;
+  units::meter_t Norm() const;
 
   /**
    * Applies a rotation to the translation in 2d space.
@@ -190,7 +192,7 @@ class Translation2d {
   Translation2d& operator/=(double scalar);
 
  private:
-  double m_x = 0;
-  double m_y = 0;
+  units::meter_t m_x = 0_m;
+  units::meter_t m_y = 0_m;
 };
 }  // namespace frc

wpilibc/src/main/native/include/frc/geometry/Twist2d.h

@@ -6,6 +6,7 @@
 /*----------------------------------------------------------------------------*/
 
 #pragma once
+#include <units/units.h>
 
 namespace frc {
 /**
@@ -19,16 +20,16 @@ struct Twist2d {
   /**
    * Linear "dx" component
    */
-  double dx = 0;
+  units::meter_t dx = 0_m;
 
   /**
    * Linear "dy" component
    */
-  double dy = 0;
+  units::meter_t dy = 0_m;
 
   /**
    * Angular "dtheta" component (radians)
    */
-  double dtheta = 0;
+  units::radian_t dtheta = 0_rad;
 };
 }  // namespace frc