[wpimath] Make geometry classes constexpr (#7222)

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

@@ -4,21 +4,24 @@
 
 #pragma once
 
+#include <algorithm>
 #include <initializer_list>
 #include <span>
 #include <utility>
 
+#include <gcem.hpp>
 #include <wpi/SymbolExports.h>
 #include <wpi/json_fwd.h>
 
 #include "frc/geometry/Rotation2d.h"
-#include "frc/geometry/Transform2d.h"
 #include "frc/geometry/Translation2d.h"
 #include "frc/geometry/Twist2d.h"
 #include "units/length.h"
 
 namespace frc {
 
+class Transform2d;
+
 /**
  * Represents a 2D pose containing translational and rotational elements.
  */
@@ -74,12 +77,12 @@ class WPILIB_DLLEXPORT Pose2d {
    * @param other The initial pose of the transformation.
    * @return The transform that maps the other pose to the current pose.
    */
-  Transform2d operator-(const Pose2d& other) const;
+  constexpr Transform2d operator-(const Pose2d& other) const;
 
   /**
    * Checks equality between this Pose2d and another object.
    */
-  bool operator==(const Pose2d&) const = default;
+  constexpr bool operator==(const Pose2d&) const = default;
 
   /**
    * Returns the underlying translation.
@@ -150,10 +153,7 @@ class WPILIB_DLLEXPORT Pose2d {
    *
    * @return The transformed pose.
    */
-  constexpr Pose2d TransformBy(const Transform2d& other) const {
-    return {m_translation + (other.Translation().RotateBy(m_rotation)),
-            other.Rotation() + m_rotation};
-  }
+  constexpr Pose2d TransformBy(const Transform2d& other) const;
 
   /**
    * Returns the current pose relative to the given pose.
@@ -167,7 +167,7 @@ class WPILIB_DLLEXPORT Pose2d {
    *
    * @return The current pose relative to the new origin pose.
    */
-  Pose2d RelativeTo(const Pose2d& other) const;
+  constexpr Pose2d RelativeTo(const Pose2d& other) const;
 
   /**
    * Obtain a new Pose2d from a (constant curvature) velocity.
@@ -190,7 +190,7 @@ class WPILIB_DLLEXPORT Pose2d {
    *
    * @return The new pose of the robot.
    */
-  Pose2d Exp(const Twist2d& twist) const;
+  constexpr Pose2d Exp(const Twist2d& twist) const;
 
   /**
    * Returns a Twist2d that maps this pose to the end pose. If c is the output
@@ -200,21 +200,51 @@ class WPILIB_DLLEXPORT Pose2d {
    *
    * @return The twist that maps this to end.
    */
-  Twist2d Log(const Pose2d& end) const;
+  constexpr Twist2d Log(const Pose2d& end) const;
 
   /**
    * Returns the nearest Pose2d from a collection of poses
    * @param poses The collection of poses.
    * @return The nearest Pose2d from the collection.
    */
-  Pose2d Nearest(std::span<const Pose2d> poses) const;
+  constexpr Pose2d Nearest(std::span<const Pose2d> poses) const {
+    return *std::min_element(
+        poses.begin(), poses.end(), [this](const Pose2d& a, const Pose2d& b) {
+          auto aDistance = this->Translation().Distance(a.Translation());
+          auto bDistance = this->Translation().Distance(b.Translation());
+
+          // If the distances are equal sort by difference in rotation
+          if (aDistance == bDistance) {
+            return gcem::abs(
+                       (this->Rotation() - a.Rotation()).Radians().value()) <
+                   gcem::abs(
+                       (this->Rotation() - b.Rotation()).Radians().value());
+          }
+          return aDistance < bDistance;
+        });
+  }
 
   /**
    * Returns the nearest Pose2d from a collection of poses
    * @param poses The collection of poses.
    * @return The nearest Pose2d from the collection.
    */
-  Pose2d Nearest(std::initializer_list<Pose2d> poses) const;
+  constexpr Pose2d Nearest(std::initializer_list<Pose2d> poses) const {
+    return *std::min_element(
+        poses.begin(), poses.end(), [this](const Pose2d& a, const Pose2d& b) {
+          auto aDistance = this->Translation().Distance(a.Translation());
+          auto bDistance = this->Translation().Distance(b.Translation());
+
+          // If the distances are equal sort by difference in rotation
+          if (aDistance == bDistance) {
+            return gcem::abs(
+                       (this->Rotation() - a.Rotation()).Radians().value()) <
+                   gcem::abs(
+                       (this->Rotation() - b.Rotation()).Radians().value());
+          }
+          return aDistance < bDistance;
+        });
+  }
 
  private:
   Translation2d m_translation;
@@ -233,3 +263,71 @@ void from_json(const wpi::json& json, Pose2d& pose);
 #include "frc/geometry/proto/Pose2dProto.h"
 #endif
 #include "frc/geometry/struct/Pose2dStruct.h"
+
+#include "frc/geometry/Transform2d.h"
+
+namespace frc {
+
+constexpr Transform2d Pose2d::operator-(const Pose2d& other) const {
+  const auto pose = this->RelativeTo(other);
+  return Transform2d{pose.Translation(), pose.Rotation()};
+}
+
+constexpr Pose2d Pose2d::TransformBy(const frc::Transform2d& other) const {
+  return {m_translation + (other.Translation().RotateBy(m_rotation)),
+          other.Rotation() + m_rotation};
+}
+
+constexpr Pose2d Pose2d::RelativeTo(const Pose2d& other) const {
+  const Transform2d transform{other, *this};
+  return {transform.Translation(), transform.Rotation()};
+}
+
+constexpr Pose2d Pose2d::Exp(const Twist2d& twist) const {
+  const auto dx = twist.dx;
+  const auto dy = twist.dy;
+  const auto dtheta = twist.dtheta.value();
+
+  const auto sinTheta = gcem::sin(dtheta);
+  const auto cosTheta = gcem::cos(dtheta);
+
+  double s, c;
+  if (gcem::abs(dtheta) < 1E-9) {
+    s = 1.0 - 1.0 / 6.0 * dtheta * dtheta;
+    c = 0.5 * dtheta;
+  } else {
+    s = sinTheta / dtheta;
+    c = (1 - cosTheta) / dtheta;
+  }
+
+  const Transform2d transform{Translation2d{dx * s - dy * c, dx * c + dy * s},
+                              Rotation2d{cosTheta, sinTheta}};
+
+  return *this + transform;
+}
+
+constexpr Twist2d Pose2d::Log(const Pose2d& end) const {
+  const auto transform = end.RelativeTo(*this);
+  const auto dtheta = transform.Rotation().Radians().value();
+  const auto halfDtheta = dtheta / 2.0;
+
+  const auto cosMinusOne = transform.Rotation().Cos() - 1;
+
+  double halfThetaByTanOfHalfDtheta;
+
+  if (gcem::abs(cosMinusOne) < 1E-9) {
+    halfThetaByTanOfHalfDtheta = 1.0 - 1.0 / 12.0 * dtheta * dtheta;
+  } else {
+    halfThetaByTanOfHalfDtheta =
+        -(halfDtheta * transform.Rotation().Sin()) / cosMinusOne;
+  }
+
+  const Translation2d translationPart =
+      transform.Translation().RotateBy(
+          {halfThetaByTanOfHalfDtheta, -halfDtheta}) *
+      gcem::hypot(halfThetaByTanOfHalfDtheta, halfDtheta);
+
+  return {translationPart.X(), translationPart.Y(), units::radian_t{dtheta}};
+}
+
+}  // namespace frc