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

wpilibc/src/main/native/cpp/geometry/Pose2d.cpp

@@ -14,7 +14,7 @@ using namespace frc;
 Pose2d::Pose2d(Translation2d translation, Rotation2d rotation)
     : m_translation(translation), m_rotation(rotation) {}
 
-Pose2d::Pose2d(double x, double y, Rotation2d rotation)
+Pose2d::Pose2d(units::meter_t x, units::meter_t y, Rotation2d rotation)
     : m_translation(x, y), m_rotation(rotation) {}
 
 Pose2d Pose2d::operator+(const Transform2d& other) const {
@@ -40,7 +40,7 @@ Pose2d Pose2d::RelativeTo(const Pose2d& other) const {
 Pose2d Pose2d::Exp(const Twist2d& twist) const {
   const auto dx = twist.dx;
   const auto dy = twist.dy;
-  const auto dtheta = twist.dtheta;
+  const auto dtheta = twist.dtheta.to<double>();
 
   const auto sinTheta = std::sin(dtheta);
   const auto cosTheta = std::cos(dtheta);

wpilibc/src/main/native/cpp/geometry/Rotation2d.cpp

@@ -11,8 +11,10 @@
 
 using namespace frc;
 
-Rotation2d::Rotation2d(double value)
-    : m_value(value), m_cos(std::cos(value)), m_sin(std::sin(value)) {}
+Rotation2d::Rotation2d(units::radian_t value)
+    : m_value(value),
+      m_cos(units::math::cos(value)),
+      m_sin(units::math::sin(value)) {}
 
 Rotation2d::Rotation2d(double x, double y) {
   const auto magnitude = std::hypot(x, y);
@@ -23,11 +25,7 @@ Rotation2d::Rotation2d(double x, double y) {
     m_sin = 0.0;
     m_cos = 1.0;
   }
-  m_value = std::atan2(m_sin, m_cos);
-}
-
-Rotation2d Rotation2d::FromDegrees(double degrees) {
-  return Rotation2d(Deg2Rad(degrees));
+  m_value = units::radian_t(std::atan2(m_sin, m_cos));
 }
 
 Rotation2d Rotation2d::operator+(const Rotation2d& other) const {
@@ -39,7 +37,7 @@ Rotation2d& Rotation2d::operator+=(const Rotation2d& other) {
   double sin = Cos() * other.Sin() + Sin() * other.Cos();
   m_cos = cos;
   m_sin = sin;
-  m_value = std::atan2(m_sin, m_cos);
+  m_value = units::radian_t(std::atan2(m_sin, m_cos));
   return *this;
 }
 

wpilibc/src/main/native/cpp/geometry/Translation2d.cpp

@@ -7,17 +7,18 @@
 
 #include "frc/geometry/Translation2d.h"
 
-#include <cmath>
-
 using namespace frc;
 
-Translation2d::Translation2d(double x, double y) : m_x(x), m_y(y) {}
+Translation2d::Translation2d(units::meter_t x, units::meter_t y)
+    : m_x(x), m_y(y) {}
 
-double Translation2d::Distance(const Translation2d& other) const {
-  return std::hypot(other.m_x - m_x, other.m_y - m_y);
+units::meter_t Translation2d::Distance(const Translation2d& other) const {
+  return units::math::hypot(other.m_x - m_x, other.m_y - m_y);
 }
 
-double Translation2d::Norm() const { return std::hypot(m_x, m_y); }
+units::meter_t Translation2d::Norm() const {
+  return units::math::hypot(m_x, m_y);
+}
 
 Translation2d Translation2d::RotateBy(const Rotation2d& other) const {
   return {m_x * other.Cos() - m_y * other.Sin(),

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