[wpimath] Increase constexpr support in geometry data types (#4231)

This uses std::is_constant_evaluated() to conditionally use the gcem library for constexpr calculations.

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

@@ -32,7 +32,7 @@ class WPILIB_DLLEXPORT Pose2d {
    * @param translation The translational component of the pose.
    * @param rotation The rotational component of the pose.
    */
-  Pose2d(Translation2d translation, Rotation2d rotation);
+  constexpr Pose2d(Translation2d translation, Rotation2d rotation);
 
   /**
    * Constructs a pose with x and y translations instead of a separate
@@ -42,7 +42,7 @@ class WPILIB_DLLEXPORT Pose2d {
    * @param y The y component of the translational component of the pose.
    * @param rotation The rotational component of the pose.
    */
-  Pose2d(units::meter_t x, units::meter_t y, Rotation2d rotation);
+  constexpr Pose2d(units::meter_t x, units::meter_t y, Rotation2d rotation);
 
   /**
    * Transforms the pose by the given transformation and returns the new
@@ -58,7 +58,7 @@ class WPILIB_DLLEXPORT Pose2d {
    *
    * @return The transformed pose.
    */
-  Pose2d operator+(const Transform2d& other) const;
+  constexpr Pose2d operator+(const Transform2d& other) const;
 
   /**
    * Returns the Transform2d that maps the one pose to another.
@@ -89,28 +89,28 @@ class WPILIB_DLLEXPORT Pose2d {
    *
    * @return Reference to the translational component of the pose.
    */
-  const Translation2d& Translation() const { return m_translation; }
+  constexpr const Translation2d& Translation() const { return m_translation; }
 
   /**
    * Returns the X component of the pose's translation.
    *
    * @return The x component of the pose's translation.
    */
-  units::meter_t X() const { return m_translation.X(); }
+  constexpr units::meter_t X() const { return m_translation.X(); }
 
   /**
    * Returns the Y component of the pose's translation.
    *
    * @return The y component of the pose's translation.
    */
-  units::meter_t Y() const { return m_translation.Y(); }
+  constexpr units::meter_t Y() const { return m_translation.Y(); }
 
   /**
    * Returns the underlying rotation.
    *
    * @return Reference to the rotational component of the pose.
    */
-  const Rotation2d& Rotation() const { return m_rotation; }
+  constexpr const Rotation2d& Rotation() const { return m_rotation; }
 
   /**
    * Multiplies the current pose by a scalar.
@@ -119,7 +119,7 @@ class WPILIB_DLLEXPORT Pose2d {
    *
    * @return The new scaled Pose2d.
    */
-  Pose2d operator*(double scalar) const;
+  constexpr Pose2d operator*(double scalar) const;
 
   /**
    * Divides the current pose by a scalar.
@@ -128,7 +128,7 @@ class WPILIB_DLLEXPORT Pose2d {
    *
    * @return The new scaled Pose2d.
    */
-  Pose2d operator/(double scalar) const;
+  constexpr Pose2d operator/(double scalar) const;
 
   /**
    * Transforms the pose by the given transformation and returns the new pose.
@@ -138,7 +138,7 @@ class WPILIB_DLLEXPORT Pose2d {
    *
    * @return The transformed pose.
    */
-  Pose2d TransformBy(const Transform2d& other) const;
+  constexpr Pose2d TransformBy(const Transform2d& other) const;
 
   /**
    * Returns the other pose relative to the current pose.
@@ -199,3 +199,5 @@ WPILIB_DLLEXPORT
 void from_json(const wpi::json& json, Pose2d& pose);
 
 }  // namespace frc
+
+#include "Pose2d.inc"

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

@@ -0,0 +1,39 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <utility>
+
+#include "frc/geometry/Pose2d.h"
+#include "frc/geometry/Rotation2d.h"
+#include "units/length.h"
+
+namespace frc {
+
+constexpr Pose2d::Pose2d(Translation2d translation, Rotation2d rotation)
+    : m_translation(std::move(translation)), m_rotation(std::move(rotation)) {}
+
+constexpr Pose2d::Pose2d(units::meter_t x, units::meter_t y,
+                         Rotation2d rotation)
+    : m_translation(x, y), m_rotation(std::move(rotation)) {}
+
+constexpr Pose2d Pose2d::operator+(const Transform2d& other) const {
+  return TransformBy(other);
+}
+
+constexpr Pose2d Pose2d::operator*(double scalar) const {
+  return Pose2d{m_translation * scalar, m_rotation * scalar};
+}
+
+constexpr Pose2d Pose2d::operator/(double scalar) const {
+  return *this * (1.0 / scalar);
+}
+
+constexpr Pose2d Pose2d::TransformBy(const Transform2d& other) const {
+  return {m_translation + (other.Translation().RotateBy(m_rotation)),
+          m_rotation + other.Rotation()};
+}
+
+}  // namespace frc

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

@@ -30,14 +30,14 @@ class WPILIB_DLLEXPORT Rotation2d {
    *
    * @param value The value of the angle in radians.
    */
-  Rotation2d(units::radian_t value);  // NOLINT
+  constexpr Rotation2d(units::radian_t value);  // NOLINT
 
   /**
    * Constructs a Rotation2d with the given degree value.
    *
    * @param value The value of the angle in degrees.
    */
-  Rotation2d(units::degree_t value);  // NOLINT
+  constexpr Rotation2d(units::degree_t value);  // NOLINT
 
   /**
    * Constructs a Rotation2d with the given x and y (cosine and sine)
@@ -46,7 +46,7 @@ class WPILIB_DLLEXPORT Rotation2d {
    * @param x The x component or cosine of the rotation.
    * @param y The y component or sine of the rotation.
    */
-  Rotation2d(double x, double y);
+  constexpr Rotation2d(double x, double y);
 
   /**
    * Adds two rotations together, with the result being bounded between -pi and
@@ -59,7 +59,7 @@ class WPILIB_DLLEXPORT Rotation2d {
    *
    * @return The sum of the two rotations.
    */
-  Rotation2d operator+(const Rotation2d& other) const;
+  constexpr Rotation2d operator+(const Rotation2d& other) const;
 
   /**
    * Subtracts the new rotation from the current rotation and returns the new
@@ -72,7 +72,7 @@ class WPILIB_DLLEXPORT Rotation2d {
    *
    * @return The difference between the two rotations.
    */
-  Rotation2d operator-(const Rotation2d& other) const;
+  constexpr Rotation2d operator-(const Rotation2d& other) const;
 
   /**
    * Takes the inverse of the current rotation. This is simply the negative of
@@ -80,7 +80,7 @@ class WPILIB_DLLEXPORT Rotation2d {
    *
    * @return The inverse of the current rotation.
    */
-  Rotation2d operator-() const;
+  constexpr Rotation2d operator-() const;
 
   /**
    * Multiplies the current rotation by a scalar.
@@ -89,7 +89,7 @@ class WPILIB_DLLEXPORT Rotation2d {
    *
    * @return The new scaled Rotation2d.
    */
-  Rotation2d operator*(double scalar) const;
+  constexpr Rotation2d operator*(double scalar) const;
 
   /**
    * Divides the current rotation by a scalar.
@@ -98,7 +98,7 @@ class WPILIB_DLLEXPORT Rotation2d {
    *
    * @return The new scaled Rotation2d.
    */
-  Rotation2d operator/(double scalar) const;
+  constexpr Rotation2d operator/(double scalar) const;
 
   /**
    * Checks equality between this Rotation2d and another object.
@@ -106,7 +106,7 @@ class WPILIB_DLLEXPORT Rotation2d {
    * @param other The other object.
    * @return Whether the two objects are equal.
    */
-  bool operator==(const Rotation2d& other) const;
+  constexpr bool operator==(const Rotation2d& other) const;
 
   /**
    * Checks inequality between this Rotation2d and another object.
@@ -114,7 +114,7 @@ class WPILIB_DLLEXPORT Rotation2d {
    * @param other The other object.
    * @return Whether the two objects are not equal.
    */
-  bool operator!=(const Rotation2d& other) const;
+  constexpr bool operator!=(const Rotation2d& other) const;
 
   /**
    * Adds the new rotation to the current rotation using a rotation matrix.
@@ -129,42 +129,42 @@ class WPILIB_DLLEXPORT Rotation2d {
    *
    * @return The new rotated Rotation2d.
    */
-  Rotation2d RotateBy(const Rotation2d& other) const;
+  constexpr Rotation2d RotateBy(const Rotation2d& other) const;
 
   /**
    * Returns the radian value of the rotation.
    *
    * @return The radian value of the rotation.
    */
-  units::radian_t Radians() const { return m_value; }
+  constexpr units::radian_t Radians() const { return m_value; }
 
   /**
    * Returns the degree value of the rotation.
    *
    * @return The degree value of the rotation.
    */
-  units::degree_t Degrees() const { return m_value; }
+  constexpr units::degree_t Degrees() const { return m_value; }
 
   /**
    * Returns the cosine of the rotation.
    *
    * @return The cosine of the rotation.
    */
-  double Cos() const { return m_cos; }
+  constexpr double Cos() const { return m_cos; }
 
   /**
    * Returns the sine of the rotation.
    *
    * @return The sine of the rotation.
    */
-  double Sin() const { return m_sin; }
+  constexpr double Sin() const { return m_sin; }
 
   /**
    * Returns the tangent of the rotation.
    *
    * @return The tangent of the rotation.
    */
-  double Tan() const { return m_sin / m_cos; }
+  constexpr double Tan() const { return Sin() / Cos(); }
 
  private:
   units::radian_t m_value = 0_rad;
@@ -179,3 +179,5 @@ WPILIB_DLLEXPORT
 void from_json(const wpi::json& json, Rotation2d& rotation);
 
 }  // namespace frc
+
+#include "Rotation2d.inc"

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

@@ -0,0 +1,74 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <type_traits>
+
+#include "frc/geometry/Rotation2d.h"
+#include "gcem.hpp"
+#include "units/angle.h"
+
+namespace frc {
+
+constexpr Rotation2d::Rotation2d(units::radian_t value)
+    : 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}) {}
+
+constexpr Rotation2d::Rotation2d(double x, double y) {
+  const auto magnitude = gcem::hypot(x, y);
+  if (magnitude > 1e-6) {
+    m_sin = y / magnitude;
+    m_cos = x / magnitude;
+  } else {
+    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_value};
+}
+
+constexpr Rotation2d Rotation2d::operator*(double scalar) const {
+  return Rotation2d(m_value * scalar);
+}
+
+constexpr Rotation2d Rotation2d::operator+(const Rotation2d& other) const {
+  return RotateBy(other);
+}
+
+constexpr Rotation2d Rotation2d::operator-(const Rotation2d& other) const {
+  return *this + -other;
+}
+
+constexpr Rotation2d Rotation2d::operator/(double scalar) const {
+  return *this * (1.0 / scalar);
+}
+
+constexpr bool Rotation2d::operator==(const Rotation2d& other) const {
+  return (std::is_constant_evaluated()
+              ? gcem::hypot(Cos() - other.Cos(), Sin() - other.Sin())
+              : std::hypot(Cos() - other.Cos(), Sin() - other.Sin())) < 1E-9;
+}
+
+constexpr bool Rotation2d::operator!=(const Rotation2d& other) const {
+  return !operator==(other);
+}
+
+constexpr Rotation2d Rotation2d::RotateBy(const Rotation2d& other) const {
+  return {Cos() * other.Cos() - Sin() * other.Sin(),
+          Cos() * other.Sin() + Sin() * other.Cos()};
+}
+
+}  // namespace frc

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

@@ -31,7 +31,7 @@ class WPILIB_DLLEXPORT Transform2d {
    * @param translation Translational component of the transform.
    * @param rotation Rotational component of the transform.
    */
-  Transform2d(Translation2d translation, Rotation2d rotation);
+  constexpr Transform2d(Translation2d translation, Rotation2d rotation);
 
   /**
    * Constructs the identity transform -- maps an initial pose to itself.
@@ -43,35 +43,35 @@ class WPILIB_DLLEXPORT Transform2d {
    *
    * @return Reference to the translational component of the transform.
    */
-  const Translation2d& Translation() const { return m_translation; }
+  constexpr const Translation2d& Translation() const { return m_translation; }
 
   /**
    * Returns the X component of the transformation's translation.
    *
    * @return The x component of the transformation's translation.
    */
-  units::meter_t X() const { return m_translation.X(); }
+  constexpr units::meter_t X() const { return m_translation.X(); }
 
   /**
    * Returns the Y component of the transformation's translation.
    *
    * @return The y component of the transformation's translation.
    */
-  units::meter_t Y() const { return m_translation.Y(); }
+  constexpr units::meter_t Y() const { return m_translation.Y(); }
 
   /**
    * Returns the rotational component of the transformation.
    *
    * @return Reference to the rotational component of the transform.
    */
-  const Rotation2d& Rotation() const { return m_rotation; }
+  constexpr const Rotation2d& Rotation() const { return m_rotation; }
 
   /**
    * Invert the transformation. This is useful for undoing a transformation.
    *
    * @return The inverted transformation.
    */
-  Transform2d Inverse() const;
+  constexpr Transform2d Inverse() const;
 
   /**
    * Multiplies the transform by the scalar.
@@ -79,7 +79,7 @@ class WPILIB_DLLEXPORT Transform2d {
    * @param scalar The scalar.
    * @return The scaled Transform2d.
    */
-  Transform2d operator*(double scalar) const {
+  constexpr Transform2d operator*(double scalar) const {
     return Transform2d(m_translation * scalar, m_rotation * scalar);
   }
 
@@ -89,7 +89,9 @@ class WPILIB_DLLEXPORT Transform2d {
    * @param scalar The scalar.
    * @return The scaled Transform2d.
    */
-  Transform2d operator/(double scalar) const { return *this * (1.0 / scalar); }
+  constexpr Transform2d operator/(double scalar) const {
+    return *this * (1.0 / scalar);
+  }
 
   /**
    * Composes two transformations.
@@ -120,3 +122,5 @@ class WPILIB_DLLEXPORT Transform2d {
   Rotation2d m_rotation;
 };
 }  // namespace frc
+
+#include "Transform2d.inc"

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

@@ -0,0 +1,26 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <utility>
+
+#include "frc/geometry/Rotation2d.h"
+#include "frc/geometry/Transform2d.h"
+#include "frc/geometry/Translation2d.h"
+
+namespace frc {
+
+constexpr Transform2d::Transform2d(Translation2d translation,
+                                   Rotation2d rotation)
+    : m_translation(std::move(translation)), m_rotation(std::move(rotation)) {}
+
+constexpr Transform2d Transform2d::Inverse() const {
+  // We are rotating the difference between the translations
+  // using a clockwise rotation matrix. This transforms the global
+  // delta into a local delta (relative to the initial pose).
+  return Transform2d{(-Translation()).RotateBy(-Rotation()), -Rotation()};
+}
+
+}  // namespace frc

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

@@ -37,7 +37,7 @@ class WPILIB_DLLEXPORT Translation2d {
    * @param x The x component of the translation.
    * @param y The y component of the translation.
    */
-  Translation2d(units::meter_t x, units::meter_t y);
+  constexpr Translation2d(units::meter_t x, units::meter_t y);
 
   /**
    * Constructs a Translation2d with the provided distance and angle. This is
@@ -46,7 +46,7 @@ class WPILIB_DLLEXPORT Translation2d {
    * @param distance The distance from the origin to the end of the translation.
    * @param angle The angle between the x-axis and the translation vector.
    */
-  Translation2d(units::meter_t distance, const Rotation2d& angle);
+  constexpr Translation2d(units::meter_t distance, const Rotation2d& angle);
 
   /**
    * Calculates the distance between two translations in 2D space.
@@ -64,14 +64,14 @@ class WPILIB_DLLEXPORT Translation2d {
    *
    * @return The X component of the translation.
    */
-  units::meter_t X() const { return m_x; }
+  constexpr units::meter_t X() const { return m_x; }
 
   /**
    * Returns the Y component of the translation.
    *
    * @return The Y component of the translation.
    */
-  units::meter_t Y() const { return m_y; }
+  constexpr units::meter_t Y() const { return m_y; }
 
   /**
    * Returns the norm, or distance from the origin to the translation.
@@ -85,7 +85,7 @@ class WPILIB_DLLEXPORT Translation2d {
    *
    * @return The angle of the translation
    */
-  Rotation2d Angle() const;
+  constexpr Rotation2d Angle() const;
 
   /**
    * Applies a rotation to the translation in 2D space.
@@ -105,7 +105,7 @@ class WPILIB_DLLEXPORT Translation2d {
    *
    * @return The new rotated translation.
    */
-  Translation2d RotateBy(const Rotation2d& other) const;
+  constexpr Translation2d RotateBy(const Rotation2d& other) const;
 
   /**
    * Returns the sum of two translations in 2D space.
@@ -117,7 +117,7 @@ class WPILIB_DLLEXPORT Translation2d {
    *
    * @return The sum of the translations.
    */
-  Translation2d operator+(const Translation2d& other) const;
+  constexpr Translation2d operator+(const Translation2d& other) const;
 
   /**
    * Returns the difference between two translations.
@@ -129,7 +129,7 @@ class WPILIB_DLLEXPORT Translation2d {
    *
    * @return The difference between the two translations.
    */
-  Translation2d operator-(const Translation2d& other) const;
+  constexpr Translation2d operator-(const Translation2d& other) const;
 
   /**
    * Returns the inverse of the current translation. This is equivalent to
@@ -138,7 +138,7 @@ class WPILIB_DLLEXPORT Translation2d {
    *
    * @return The inverse of the current translation.
    */
-  Translation2d operator-() const;
+  constexpr Translation2d operator-() const;
 
   /**
    * Returns the translation multiplied by a scalar.
@@ -149,7 +149,7 @@ class WPILIB_DLLEXPORT Translation2d {
    *
    * @return The scaled translation.
    */
-  Translation2d operator*(double scalar) const;
+  constexpr Translation2d operator*(double scalar) const;
 
   /**
    * Returns the translation divided by a scalar.
@@ -160,7 +160,7 @@ class WPILIB_DLLEXPORT Translation2d {
    *
    * @return The scaled translation.
    */
-  Translation2d operator/(double scalar) const;
+  constexpr Translation2d operator/(double scalar) const;
 
   /**
    * Checks equality between this Translation2d and another object.
@@ -190,3 +190,5 @@ WPILIB_DLLEXPORT
 void from_json(const wpi::json& json, Translation2d& state);
 
 }  // namespace frc
+
+#include "Translation2d.inc"

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

@@ -0,0 +1,50 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include "frc/geometry/Translation2d.h"
+#include "units/length.h"
+
+namespace frc {
+
+constexpr Translation2d::Translation2d(units::meter_t x, units::meter_t y)
+    : m_x(x), m_y(y) {}
+
+constexpr Translation2d::Translation2d(units::meter_t distance,
+                                       const Rotation2d& angle)
+    : m_x(distance * angle.Cos()), m_y(distance * angle.Sin()) {}
+
+constexpr Rotation2d Translation2d::Angle() const {
+  return Rotation2d{m_x.value(), m_y.value()};
+}
+
+constexpr Translation2d Translation2d::RotateBy(const Rotation2d& other) const {
+  return {m_x * other.Cos() - m_y * other.Sin(),
+          m_x * other.Sin() + m_y * other.Cos()};
+}
+
+constexpr Translation2d Translation2d::operator+(
+    const Translation2d& other) const {
+  return {X() + other.X(), Y() + other.Y()};
+}
+
+constexpr Translation2d Translation2d::operator-(
+    const Translation2d& other) const {
+  return *this + -other;
+}
+
+constexpr Translation2d Translation2d::operator-() const {
+  return {-m_x, -m_y};
+}
+
+constexpr Translation2d Translation2d::operator*(double scalar) const {
+  return {scalar * m_x, scalar * m_y};
+}
+
+constexpr Translation2d Translation2d::operator/(double scalar) const {
+  return operator*(1.0 / scalar);
+}
+
+}  // namespace frc

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

@@ -35,7 +35,7 @@ class WPILIB_DLLEXPORT Translation3d {
    * @param y The y component of the translation.
    * @param z The z component of the translation.
    */
-  Translation3d(units::meter_t x, units::meter_t y, units::meter_t z);
+  constexpr Translation3d(units::meter_t x, units::meter_t y, units::meter_t z);
 
   /**
    * Constructs a Translation3d with the provided distance and angle. This is
@@ -63,21 +63,21 @@ class WPILIB_DLLEXPORT Translation3d {
    *
    * @return The Z component of the translation.
    */
-  units::meter_t X() const { return m_x; }
+  constexpr units::meter_t X() const { return m_x; }
 
   /**
    * Returns the Y component of the translation.
    *
    * @return The Y component of the translation.
    */
-  units::meter_t Y() const { return m_y; }
+  constexpr units::meter_t Y() const { return m_y; }
 
   /**
    * Returns the Z component of the translation.
    *
    * @return The Z component of the translation.
    */
-  units::meter_t Z() const { return m_z; }
+  constexpr units::meter_t Z() const { return m_z; }
 
   /**
    * Returns the norm, or distance from the origin to the translation.
@@ -102,7 +102,7 @@ class WPILIB_DLLEXPORT Translation3d {
    * Returns a Translation2d representing this Translation3d projected into the
    * X-Y plane.
    */
-  Translation2d ToTranslation2d() const;
+  constexpr Translation2d ToTranslation2d() const;
 
   /**
    * Returns the sum of two translations in 3D space.
@@ -114,7 +114,7 @@ class WPILIB_DLLEXPORT Translation3d {
    *
    * @return The sum of the translations.
    */
-  Translation3d operator+(const Translation3d& other) const;
+  constexpr Translation3d operator+(const Translation3d& other) const;
 
   /**
    * Returns the difference between two translations.
@@ -126,7 +126,7 @@ class WPILIB_DLLEXPORT Translation3d {
    *
    * @return The difference between the two translations.
    */
-  Translation3d operator-(const Translation3d& other) const;
+  constexpr Translation3d operator-(const Translation3d& other) const;
 
   /**
    * Returns the inverse of the current translation. This is equivalent to
@@ -134,7 +134,7 @@ class WPILIB_DLLEXPORT Translation3d {
    *
    * @return The inverse of the current translation.
    */
-  Translation3d operator-() const;
+  constexpr Translation3d operator-() const;
 
   /**
    * Returns the translation multiplied by a scalar.
@@ -146,7 +146,7 @@ class WPILIB_DLLEXPORT Translation3d {
    *
    * @return The scaled translation.
    */
-  Translation3d operator*(double scalar) const;
+  constexpr Translation3d operator*(double scalar) const;
 
   /**
    * Returns the translation divided by a scalar.
@@ -158,7 +158,7 @@ class WPILIB_DLLEXPORT Translation3d {
    *
    * @return The scaled translation.
    */
-  Translation3d operator/(double scalar) const;
+  constexpr Translation3d operator/(double scalar) const;
 
   /**
    * Checks equality between this Translation3d and another object.
@@ -183,3 +183,5 @@ class WPILIB_DLLEXPORT Translation3d {
 };
 
 }  // namespace frc
+
+#include "Translation3d.inc"

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

@@ -0,0 +1,44 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include "frc/geometry/Translation2d.h"
+#include "frc/geometry/Translation3d.h"
+#include "units/length.h"
+#include "units/math.h"
+
+namespace frc {
+
+constexpr Translation3d::Translation3d(units::meter_t x, units::meter_t y,
+                                       units::meter_t z)
+    : m_x(x), m_y(y), m_z(z) {}
+
+constexpr Translation2d Translation3d::ToTranslation2d() const {
+  return Translation2d{m_x, m_y};
+}
+
+constexpr Translation3d Translation3d::operator+(
+    const Translation3d& other) const {
+  return {X() + other.X(), Y() + other.Y(), Z() + other.Z()};
+}
+
+constexpr Translation3d Translation3d::operator-(
+    const Translation3d& other) const {
+  return operator+(-other);
+}
+
+constexpr Translation3d Translation3d::operator-() const {
+  return {-m_x, -m_y, -m_z};
+}
+
+constexpr Translation3d Translation3d::operator*(double scalar) const {
+  return {scalar * m_x, scalar * m_y, scalar * m_z};
+}
+
+constexpr Translation3d Translation3d::operator/(double scalar) const {
+  return operator*(1.0 / scalar);
+}
+
+}  // namespace frc

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

@@ -60,7 +60,7 @@ struct WPILIB_DLLEXPORT Twist2d {
    * @param factor The factor by which to scale.
    * @return The scaled Twist2d.
    */
-  Twist2d operator*(double factor) const {
+  constexpr Twist2d operator*(double factor) const {
     return Twist2d{dx * factor, dy * factor, dtheta * factor};
   }
 };

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

@@ -79,7 +79,7 @@ struct WPILIB_DLLEXPORT Twist3d {
    * @param factor The factor by which to scale.
    * @return The scaled Twist3d.
    */
-  Twist3d operator*(double factor) const {
+  constexpr Twist3d operator*(double factor) const {
     return Twist3d{dx * factor, dy * factor, dz * factor,
                    rx * factor, ry * factor, rz * factor};
   }