Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-26 01:51:41 +00:00
Code Issues Packages Projects Releases Wiki Activity

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

Browse Source
This commit is contained in:
Tyler Veness
2022-05-18 10:47:46 -07:00
committed by GitHub
parent 5983434a70
commit 5876b40f08
6 changed files with 62 additions and 39 deletions
Download Patch File Download Diff File Expand all files Collapse all files

19
wpimath/src/main/java/edu/wpi/first/math/geometry/CoordinateAxis.java
View File

@@ -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<N3> 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;
}
}

13
wpimath/src/main/java/edu/wpi/first/math/geometry/CoordinateSystem.java
View File

@@ -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;
}
/**

30
wpimath/src/main/native/cpp/geometry/CoordinateAxis.cpp
View File

@@ -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;
}

21
wpimath/src/main/native/cpp/geometry/CoordinateSystem.cpp
View File

@@ -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,

12
wpimath/src/main/native/include/frc/geometry/CoordinateAxis.h
View File

@@ -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;

6
wpimath/src/main/native/include/frc/geometry/CoordinateSystem.h
View File

@@ -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.