[wpimath] Memoize CoordinateSystem and CoordinateAxis statics (#4241)

wpimath/src/main/native/cpp/geometry/CoordinateAxis.cpp

@@ -10,26 +10,32 @@ CoordinateAxis::CoordinateAxis(double x, double y, double z) : m_axis{x, y, z} {
   m_axis /= m_axis.norm();
 }
 
-CoordinateAxis CoordinateAxis::N() {
-  return CoordinateAxis{1.0, 0.0, 0.0};
+const CoordinateAxis& CoordinateAxis::N() {
+  static const CoordinateAxis instance{1.0, 0.0, 0.0};
+  return instance;
 }
 
-CoordinateAxis CoordinateAxis::S() {
-  return CoordinateAxis{-1.0, 0.0, 0.0};
+const CoordinateAxis& CoordinateAxis::S() {
+  static const CoordinateAxis instance{-1.0, 0.0, 0.0};
+  return instance;
 }
 
-CoordinateAxis CoordinateAxis::E() {
-  return CoordinateAxis{0.0, -1.0, 0.0};
+const CoordinateAxis& CoordinateAxis::E() {
+  static const CoordinateAxis instance{0.0, -1.0, 0.0};
+  return instance;
 }
 
-CoordinateAxis CoordinateAxis::W() {
-  return CoordinateAxis{0.0, 1.0, 0.0};
+const CoordinateAxis& CoordinateAxis::W() {
+  static const CoordinateAxis instance{0.0, 1.0, 0.0};
+  return instance;
 }
 
-CoordinateAxis CoordinateAxis::U() {
-  return CoordinateAxis{0.0, 0.0, 1.0};
+const CoordinateAxis& CoordinateAxis::U() {
+  static const CoordinateAxis instance{0.0, 0.0, 1.0};
+  return instance;
 }
 
-CoordinateAxis CoordinateAxis::D() {
-  return CoordinateAxis{0.0, 0.0, -1.0};
+const CoordinateAxis& CoordinateAxis::D() {
+  static const CoordinateAxis instance{0.0, 0.0, -1.0};
+  return instance;
 }

wpimath/src/main/native/cpp/geometry/CoordinateSystem.cpp

@@ -69,19 +69,22 @@ CoordinateSystem::CoordinateSystem(const CoordinateAxis& positiveX,
   m_rotation = Rotation3d{Quaternion{w, x, y, z}};
 }
 
-CoordinateSystem CoordinateSystem::NWU() {
-  return CoordinateSystem{CoordinateAxis::N(), CoordinateAxis::W(),
-                          CoordinateAxis::U()};
+const CoordinateSystem& CoordinateSystem::NWU() {
+  static const CoordinateSystem instance{
+      CoordinateAxis::N(), CoordinateAxis::W(), CoordinateAxis::U()};
+  return instance;
 }
 
-CoordinateSystem CoordinateSystem::EDN() {
-  return CoordinateSystem{CoordinateAxis::E(), CoordinateAxis::D(),
-                          CoordinateAxis::N()};
+const CoordinateSystem& CoordinateSystem::EDN() {
+  static const CoordinateSystem instance{
+      CoordinateAxis::E(), CoordinateAxis::D(), CoordinateAxis::N()};
+  return instance;
 }
 
-CoordinateSystem CoordinateSystem::NED() {
-  return CoordinateSystem{CoordinateAxis::N(), CoordinateAxis::E(),
-                          CoordinateAxis::D()};
+const CoordinateSystem& CoordinateSystem::NED() {
+  static const CoordinateSystem instance{
+      CoordinateAxis::N(), CoordinateAxis::E(), CoordinateAxis::D()};
+  return instance;
 }
 
 Pose3d CoordinateSystem::Convert(const Pose3d& pose,

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

@@ -37,32 +37,32 @@ class WPILIB_DLLEXPORT CoordinateAxis {
   /**
    * Returns a coordinate axis corresponding to +X in the NWU coordinate system.
    */
-  static CoordinateAxis N();
+  static const CoordinateAxis& N();
 
   /**
    * Returns a coordinate axis corresponding to -X in the NWU coordinate system.
    */
-  static CoordinateAxis S();
+  static const CoordinateAxis& S();
 
   /**
    * Returns a coordinate axis corresponding to -Y in the NWU coordinate system.
    */
-  static CoordinateAxis E();
+  static const CoordinateAxis& E();
 
   /**
    * Returns a coordinate axis corresponding to +Y in the NWU coordinate system.
    */
-  static CoordinateAxis W();
+  static const CoordinateAxis& W();
 
   /**
    * Returns a coordinate axis corresponding to +Z in the NWU coordinate system.
    */
-  static CoordinateAxis U();
+  static const CoordinateAxis& U();
 
   /**
    * Returns a coordinate axis corresponding to -Z in the NWU coordinate system.
    */
-  static CoordinateAxis D();
+  static const CoordinateAxis& D();
 
  private:
   friend class CoordinateSystem;

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

@@ -42,21 +42,21 @@ class WPILIB_DLLEXPORT CoordinateSystem {
    *
    * The +X axis is north, the +Y axis is west, and the +Z axis is up.
    */
-  static CoordinateSystem NWU();
+  static const CoordinateSystem& NWU();
 
   /**
    * Returns an instance of the East-Down-North (EDN) coordinate system.
    *
    * The +X axis is east, the +Y axis is down, and the +Z axis is north.
    */
-  static CoordinateSystem EDN();
+  static const CoordinateSystem& EDN();
 
   /**
    * Returns an instance of the NED coordinate system.
    *
    * The +X axis is north, the +Y axis is east, and the +Z axis is down.
    */
-  static CoordinateSystem NED();
+  static const CoordinateSystem& NED();
 
   /**
    * Converts the given pose from one coordinate system to another.