[wpimath] Make various classes constexpr (wpilibsuite#7237)

wpimath/src/main/native/cpp/StateSpaceUtil.cpp

@@ -6,28 +6,11 @@
 
 namespace frc {
 
-Eigen::Vector3d PoseTo3dVector(const Pose2d& pose) {
-  return Eigen::Vector3d{pose.Translation().X().value(),
-                         pose.Translation().Y().value(),
-                         pose.Rotation().Radians().value()};
-}
-
-Eigen::Vector4d PoseTo4dVector(const Pose2d& pose) {
-  return Eigen::Vector4d{pose.Translation().X().value(),
-                         pose.Translation().Y().value(), pose.Rotation().Cos(),
-                         pose.Rotation().Sin()};
-}
-
 template bool IsStabilizable<1, 1>(const Matrixd<1, 1>& A,
                                    const Matrixd<1, 1>& B);
 template bool IsStabilizable<2, 1>(const Matrixd<2, 2>& A,
                                    const Matrixd<2, 1>& B);
 template bool IsStabilizable<Eigen::Dynamic, Eigen::Dynamic>(
     const Eigen::MatrixXd& A, const Eigen::MatrixXd& B);
 
-Eigen::Vector3d PoseToVector(const Pose2d& pose) {
-  return Eigen::Vector3d{pose.X().value(), pose.Y().value(),
-                         pose.Rotation().Radians().value()};
-}
-
 }  // namespace frc

wpimath/src/main/native/include/frc/ComputerVisionUtil.h

@@ -30,8 +30,12 @@ namespace frc {
  * @return The robot's field-relative pose.
  */
 WPILIB_DLLEXPORT
-frc::Pose3d ObjectToRobotPose(const frc::Pose3d& objectInField,
-                              const frc::Transform3d& cameraToObject,
-                              const frc::Transform3d& robotToCamera);
+constexpr frc::Pose3d ObjectToRobotPose(const frc::Pose3d& objectInField,
+                                        const frc::Transform3d& cameraToObject,
+                                        const frc::Transform3d& robotToCamera) {
+  const auto objectToCamera = cameraToObject.Inverse();
+  const auto cameraToRobot = robotToCamera.Inverse();
+  return objectInField + objectToCamera + cameraToRobot;
+}
 
 }  // namespace frc

wpimath/src/main/native/include/frc/MathUtil.h

@@ -7,6 +7,7 @@
 #include <numbers>
 #include <type_traits>
 
+#include <gcem.hpp>
 #include <wpi/SymbolExports.h>
 
 #include "units/angle.h"
@@ -28,10 +29,10 @@ namespace frc {
  */
 template <typename T>
   requires std::is_arithmetic_v<T> || units::traits::is_unit_t_v<T>
-T ApplyDeadband(T value, T deadband, T maxMagnitude = T{1.0}) {
+constexpr T ApplyDeadband(T value, T deadband, T maxMagnitude = T{1.0}) {
   T magnitude;
   if constexpr (std::is_arithmetic_v<T>) {
-    magnitude = std::abs(value);
+    magnitude = gcem::abs(value);
   } else {
     magnitude = units::math::abs(value);
   }

wpimath/src/main/native/include/frc/StateSpaceUtil.h

@@ -209,7 +209,11 @@ Vectord<N> MakeWhiteNoiseVector(const std::array<double, N>& stdDevs) {
  * @return The vector.
  */
 WPILIB_DLLEXPORT
-Eigen::Vector3d PoseTo3dVector(const Pose2d& pose);
+constexpr Eigen::Vector3d PoseTo3dVector(const Pose2d& pose) {
+  return Eigen::Vector3d{{pose.Translation().X().value(),
+                          pose.Translation().Y().value(),
+                          pose.Rotation().Radians().value()}};
+}
 
 /**
  * Converts a Pose2d into a vector of [x, y, std::cos(theta), std::sin(theta)].
@@ -219,7 +223,11 @@ Eigen::Vector3d PoseTo3dVector(const Pose2d& pose);
  * @return The vector.
  */
 WPILIB_DLLEXPORT
-Eigen::Vector4d PoseTo4dVector(const Pose2d& pose);
+constexpr Eigen::Vector4d PoseTo4dVector(const Pose2d& pose) {
+  return Eigen::Vector4d{{pose.Translation().X().value(),
+                          pose.Translation().Y().value(), pose.Rotation().Cos(),
+                          pose.Rotation().Sin()}};
+}
 
 /**
  * Returns true if (A, B) is a stabilizable pair.
@@ -306,7 +314,10 @@ bool IsDetectable(const Matrixd<States, States>& A,
  * @return The vector.
  */
 WPILIB_DLLEXPORT
-Eigen::Vector3d PoseToVector(const Pose2d& pose);
+constexpr Eigen::Vector3d PoseToVector(const Pose2d& pose) {
+  return Eigen::Vector3d{
+      {pose.X().value(), pose.Y().value(), pose.Rotation().Radians().value()}};
+}
 
 /**
  * Clamps input vector between system's minimum and maximum allowable input.
@@ -318,12 +329,12 @@ Eigen::Vector3d PoseToVector(const Pose2d& pose);
  * @return Clamped input vector.
  */
 template <int Inputs>
-Vectord<Inputs> ClampInputMaxMagnitude(const Vectord<Inputs>& u,
-                                       const Vectord<Inputs>& umin,
-                                       const Vectord<Inputs>& umax) {
+constexpr Vectord<Inputs> ClampInputMaxMagnitude(const Vectord<Inputs>& u,
+                                                 const Vectord<Inputs>& umin,
+                                                 const Vectord<Inputs>& umax) {
   Vectord<Inputs> result;
   for (int i = 0; i < Inputs; ++i) {
-    result(i) = std::clamp(u(i), umin(i), umax(i));
+    result.coeffRef(i) = std::clamp(u.coeff(i), umin.coeff(i), umax.coeff(i));
   }
   return result;
 }

wpimath/src/main/native/include/frc/filter/LinearFilter.h

@@ -5,13 +5,14 @@
 #pragma once
 
 #include <algorithm>
-#include <cmath>
 #include <initializer_list>
 #include <span>
 #include <stdexcept>
+#include <type_traits>
 #include <vector>
 
 #include <Eigen/QR>
+#include <gcem.hpp>
 #include <wpi/array.h>
 #include <wpi/circular_buffer.h>
 
@@ -80,7 +81,8 @@ class LinearFilter {
    * @param ffGains The "feedforward" or FIR gains.
    * @param fbGains The "feedback" or IIR gains.
    */
-  LinearFilter(std::span<const double> ffGains, std::span<const double> fbGains)
+  constexpr LinearFilter(std::span<const double> ffGains,
+                         std::span<const double> fbGains)
       : m_inputs(ffGains.size()),
         m_outputs(fbGains.size()),
         m_inputGains(ffGains.begin(), ffGains.end()),
@@ -92,10 +94,11 @@ class LinearFilter {
       m_outputs.emplace_front(0.0);
     }
 
-    static int instances = 0;
-    instances++;
-    wpi::math::MathSharedStore::ReportUsage(
-        wpi::math::MathUsageId::kFilter_Linear, instances);
+    if (!std::is_constant_evaluated()) {
+      ++instances;
+      wpi::math::MathSharedStore::ReportUsage(
+          wpi::math::MathUsageId::kFilter_Linear, instances);
+    }
   }
 
   /**
@@ -104,8 +107,8 @@ class LinearFilter {
    * @param ffGains The "feedforward" or FIR gains.
    * @param fbGains The "feedback" or IIR gains.
    */
-  LinearFilter(std::initializer_list<double> ffGains,
-               std::initializer_list<double> fbGains)
+  constexpr LinearFilter(std::initializer_list<double> ffGains,
+                         std::initializer_list<double> fbGains)
       : LinearFilter({ffGains.begin(), ffGains.end()},
                      {fbGains.begin(), fbGains.end()}) {}
 
@@ -124,9 +127,9 @@ class LinearFilter {
    * @param period       The period in seconds between samples taken by the
    *                     user.
    */
-  static LinearFilter<T> SinglePoleIIR(double timeConstant,
-                                       units::second_t period) {
-    double gain = std::exp(-period.value() / timeConstant);
+  static constexpr LinearFilter<T> SinglePoleIIR(double timeConstant,
+                                                 units::second_t period) {
+    double gain = gcem::exp(-period.value() / timeConstant);
     return LinearFilter({1.0 - gain}, {-gain});
   }
 
@@ -144,8 +147,9 @@ class LinearFilter {
    * @param period       The period in seconds between samples taken by the
    *                     user.
    */
-  static LinearFilter<T> HighPass(double timeConstant, units::second_t period) {
-    double gain = std::exp(-period.value() / timeConstant);
+  static constexpr LinearFilter<T> HighPass(double timeConstant,
+                                            units::second_t period) {
+    double gain = gcem::exp(-period.value() / timeConstant);
     return LinearFilter({gain, -gain}, {-gain});
   }
 
@@ -213,7 +217,7 @@ class LinearFilter {
     Matrixd<Samples, Samples> S;
     for (int row = 0; row < Samples; ++row) {
       for (int col = 0; col < Samples; ++col) {
-        S(row, col) = std::pow(stencil[col], row);
+        S(row, col) = gcem::pow(stencil[col], row);
       }
     }
 
@@ -224,7 +228,7 @@ class LinearFilter {
     }
 
     Vectord<Samples> a =
-        S.householderQr().solve(d) / std::pow(period.value(), Derivative);
+        S.householderQr().solve(d) / gcem::pow(period.value(), Derivative);
 
     // Reverse gains list
     std::vector<double> ffGains;
@@ -266,7 +270,7 @@ class LinearFilter {
   /**
    * Reset the filter state.
    */
-  void Reset() {
+  constexpr void Reset() {
     std::fill(m_inputs.begin(), m_inputs.end(), T{0.0});
     std::fill(m_outputs.begin(), m_outputs.end(), T{0.0});
   }
@@ -321,7 +325,8 @@ class LinearFilter {
    * @throws std::runtime_error if size of inputBuffer or outputBuffer does not
    *   match the size of ffGains and fbGains provided in the constructor.
    */
-  void Reset(std::span<const T> inputBuffer, std::span<const T> outputBuffer) {
+  constexpr void Reset(std::span<const T> inputBuffer,
+                       std::span<const T> outputBuffer) {
     // Clear buffers
     Reset();
 
@@ -346,7 +351,7 @@ class LinearFilter {
    *
    * @return The filtered value at this step
    */
-  T Calculate(T input) {
+  constexpr T Calculate(T input) {
     T retVal{0.0};
 
     // Rotate the inputs
@@ -376,7 +381,7 @@ class LinearFilter {
    *
    * @return The last value.
    */
-  T LastValue() const { return m_lastOutput; }
+  constexpr T LastValue() const { return m_lastOutput; }
 
  private:
   wpi::circular_buffer<T> m_inputs;
@@ -385,6 +390,9 @@ class LinearFilter {
   std::vector<double> m_outputGains;
   T m_lastOutput{0.0};
 
+  // Usage reporting instances
+  inline static int instances = 0;
+
   /**
    * Factorial of n.
    *