diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/CoordinateAxis.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/CoordinateAxis.java index 19f4a3f083..889ac35419 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/CoordinateAxis.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/CoordinateAxis.java @@ -10,6 +10,13 @@ import edu.wpi.first.math.numbers.N3; /** A class representing a coordinate system axis within the NWU coordinate system. */ public class CoordinateAxis { + private static final CoordinateAxis m_n = new CoordinateAxis(1.0, 0.0, 0.0); + private static final CoordinateAxis m_s = new CoordinateAxis(-1.0, 0.0, 0.0); + private static final CoordinateAxis m_e = new CoordinateAxis(0.0, -1.0, 0.0); + private static final CoordinateAxis m_w = new CoordinateAxis(0.0, 1.0, 0.0); + private static final CoordinateAxis m_u = new CoordinateAxis(0.0, 0.0, 1.0); + private static final CoordinateAxis m_d = new CoordinateAxis(0.0, 0.0, -1.0); + final Vector m_axis; /** @@ -31,7 +38,7 @@ public class CoordinateAxis { */ @SuppressWarnings("MethodName") public static CoordinateAxis N() { - return new CoordinateAxis(1.0, 0.0, 0.0); + return m_n; } /** @@ -41,7 +48,7 @@ public class CoordinateAxis { */ @SuppressWarnings("MethodName") public static CoordinateAxis S() { - return new CoordinateAxis(-1.0, 0.0, 0.0); + return m_s; } /** @@ -51,7 +58,7 @@ public class CoordinateAxis { */ @SuppressWarnings("MethodName") public static CoordinateAxis E() { - return new CoordinateAxis(0.0, -1.0, 0.0); + return m_e; } /** @@ -61,7 +68,7 @@ public class CoordinateAxis { */ @SuppressWarnings("MethodName") public static CoordinateAxis W() { - return new CoordinateAxis(0.0, 1.0, 0.0); + return m_w; } /** @@ -71,7 +78,7 @@ public class CoordinateAxis { */ @SuppressWarnings("MethodName") public static CoordinateAxis U() { - return new CoordinateAxis(0.0, 0.0, 1.0); + return m_u; } /** @@ -81,6 +88,6 @@ public class CoordinateAxis { */ @SuppressWarnings("MethodName") public static CoordinateAxis D() { - return new CoordinateAxis(0.0, 0.0, -1.0); + return m_d; } } diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/CoordinateSystem.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/CoordinateSystem.java index dae9eb8d84..449ba7438e 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/CoordinateSystem.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/CoordinateSystem.java @@ -9,6 +9,13 @@ import edu.wpi.first.math.Nat; /** A helper class that converts Pose3d objects between different standard coordinate frames. */ public class CoordinateSystem { + private static final CoordinateSystem m_nwu = + new CoordinateSystem(CoordinateAxis.N(), CoordinateAxis.W(), CoordinateAxis.U()); + private static final CoordinateSystem m_edn = + new CoordinateSystem(CoordinateAxis.E(), CoordinateAxis.D(), CoordinateAxis.N()); + private static final CoordinateSystem m_ned = + new CoordinateSystem(CoordinateAxis.N(), CoordinateAxis.E(), CoordinateAxis.D()); + // Rotation from this coordinate system to NWU coordinate system private final Rotation3d m_rotation; @@ -86,7 +93,7 @@ public class CoordinateSystem { */ @SuppressWarnings("MethodName") public static CoordinateSystem NWU() { - return new CoordinateSystem(CoordinateAxis.N(), CoordinateAxis.W(), CoordinateAxis.U()); + return m_nwu; } /** @@ -98,7 +105,7 @@ public class CoordinateSystem { */ @SuppressWarnings("MethodName") public static CoordinateSystem EDN() { - return new CoordinateSystem(CoordinateAxis.E(), CoordinateAxis.D(), CoordinateAxis.N()); + return m_edn; } /** @@ -110,7 +117,7 @@ public class CoordinateSystem { */ @SuppressWarnings("MethodName") public static CoordinateSystem NED() { - return new CoordinateSystem(CoordinateAxis.N(), CoordinateAxis.E(), CoordinateAxis.D()); + return m_ned; } /** diff --git a/wpimath/src/main/native/cpp/geometry/CoordinateAxis.cpp b/wpimath/src/main/native/cpp/geometry/CoordinateAxis.cpp index b180b3c7ba..1d9c42a2a4 100644 --- a/wpimath/src/main/native/cpp/geometry/CoordinateAxis.cpp +++ b/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; } diff --git a/wpimath/src/main/native/cpp/geometry/CoordinateSystem.cpp b/wpimath/src/main/native/cpp/geometry/CoordinateSystem.cpp index c05f927d4c..f4d200c92e 100644 --- a/wpimath/src/main/native/cpp/geometry/CoordinateSystem.cpp +++ b/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, diff --git a/wpimath/src/main/native/include/frc/geometry/CoordinateAxis.h b/wpimath/src/main/native/include/frc/geometry/CoordinateAxis.h index 1ac1a2c07a..58b966ab07 100644 --- a/wpimath/src/main/native/include/frc/geometry/CoordinateAxis.h +++ b/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; diff --git a/wpimath/src/main/native/include/frc/geometry/CoordinateSystem.h b/wpimath/src/main/native/include/frc/geometry/CoordinateSystem.h index 8dd6a81f2f..1062aa5eae 100644 --- a/wpimath/src/main/native/include/frc/geometry/CoordinateSystem.h +++ b/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.