Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-20 00:51:42 +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;
}
/**