[wpimath] Add CoordinateSystem class (#4214)

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

@@ -0,0 +1,86 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.geometry;
+
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.Vector;
+import edu.wpi.first.math.numbers.N3;
+
+/** A class representing a coordinate system axis within the NWU coordinate system. */
+public class CoordinateAxis {
+  final Vector<N3> m_axis;
+
+  /**
+   * Constructs a coordinate system axis within the NWU coordinate system and normalizes it.
+   *
+   * @param x The x component.
+   * @param y The y component.
+   * @param z The z component.
+   */
+  public CoordinateAxis(double x, double y, double z) {
+    double norm = Math.sqrt(x * x + y * y + z * z);
+    m_axis = VecBuilder.fill(x / norm, y / norm, z / norm);
+  }
+
+  /**
+   * Returns a coordinate axis corresponding to +X in the NWU coordinate system.
+   *
+   * @return A coordinate axis corresponding to +X in the NWU coordinate system.
+   */
+  @SuppressWarnings("MethodName")
+  public static CoordinateAxis N() {
+    return new CoordinateAxis(1.0, 0.0, 0.0);
+  }
+
+  /**
+   * Returns a coordinate axis corresponding to -X in the NWU coordinate system.
+   *
+   * @return A coordinate axis corresponding to -X in the NWU coordinate system.
+   */
+  @SuppressWarnings("MethodName")
+  public static CoordinateAxis S() {
+    return new CoordinateAxis(-1.0, 0.0, 0.0);
+  }
+
+  /**
+   * Returns a coordinate axis corresponding to -Y in the NWU coordinate system.
+   *
+   * @return A coordinate axis corresponding to -Y in the NWU coordinate system.
+   */
+  @SuppressWarnings("MethodName")
+  public static CoordinateAxis E() {
+    return new CoordinateAxis(0.0, -1.0, 0.0);
+  }
+
+  /**
+   * Returns a coordinate axis corresponding to +Y in the NWU coordinate system.
+   *
+   * @return A coordinate axis corresponding to +Y in the NWU coordinate system.
+   */
+  @SuppressWarnings("MethodName")
+  public static CoordinateAxis W() {
+    return new CoordinateAxis(0.0, 1.0, 0.0);
+  }
+
+  /**
+   * Returns a coordinate axis corresponding to +Z in the NWU coordinate system.
+   *
+   * @return A coordinate axis corresponding to +Z in the NWU coordinate system.
+   */
+  @SuppressWarnings("MethodName")
+  public static CoordinateAxis U() {
+    return new CoordinateAxis(0.0, 0.0, 1.0);
+  }
+
+  /**
+   * Returns a coordinate axis corresponding to -Z in the NWU coordinate system.
+   *
+   * @return A coordinate axis corresponding to -Z in the NWU coordinate system.
+   */
+  @SuppressWarnings("MethodName")
+  public static CoordinateAxis D() {
+    return new CoordinateAxis(0.0, 0.0, -1.0);
+  }
+}

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

@@ -0,0 +1,121 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.geometry;
+
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.Nat;
+
+/** A helper class that converts Pose3d objects between different standard coordinate frames. */
+public class CoordinateSystem {
+  // Rotation from this coordinate system to NWU coordinate system
+  private final Rotation3d m_rotation;
+
+  /**
+   * Constructs a coordinate system with the given cardinal directions for each axis.
+   *
+   * @param positiveX The cardinal direction of the positive x-axis.
+   * @param positiveY The cardinal direction of the positive y-axis.
+   * @param positiveZ The cardinal direction of the positive z-axis.
+   * @throws IllegalArgumentException if the coordinate system isn't special orthogonal
+   */
+  public CoordinateSystem(
+      CoordinateAxis positiveX, CoordinateAxis positiveY, CoordinateAxis positiveZ) {
+    // Construct a change of basis matrix from the source coordinate system to the
+    // NWU coordinate system. Each column vector in the change of basis matrix is
+    // one of the old basis vectors mapped to its representation in the new basis.
+    @SuppressWarnings("LocalVariableName")
+    var R = new Matrix<>(Nat.N3(), Nat.N3());
+    R.assignBlock(0, 0, positiveX.m_axis);
+    R.assignBlock(0, 1, positiveY.m_axis);
+    R.assignBlock(0, 2, positiveZ.m_axis);
+
+    // Require that the change of basis matrix is special orthogonal. This is true
+    // if the axes used are orthogonal and normalized. The Axis class already
+    // normalizes itself, so we just need to check for orthogonality.
+    if (!R.times(R.transpose()).equals(Matrix.eye(Nat.N3()))) {
+      throw new IllegalArgumentException("Coordinate system isn't special orthogonal");
+    }
+
+    // Turn change of basis matrix into a quaternion since it's a pure rotation
+    // https://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/
+    double trace = R.get(0, 0) + R.get(1, 1) + R.get(2, 2);
+    double w;
+    double x;
+    double y;
+    double z;
+
+    if (trace > 0.0) {
+      double s = 0.5 / Math.sqrt(trace + 1.0);
+      w = 0.25 / s;
+      x = (R.get(2, 1) - R.get(1, 2)) * s;
+      y = (R.get(0, 2) - R.get(2, 0)) * s;
+      z = (R.get(1, 0) - R.get(0, 1)) * s;
+    } else {
+      if (R.get(0, 0) > R.get(1, 1) && R.get(0, 0) > R.get(2, 2)) {
+        double s = 2.0 * Math.sqrt(1.0 + R.get(0, 0) - R.get(1, 1) - R.get(2, 2));
+        w = (R.get(2, 1) - R.get(1, 2)) / s;
+        x = 0.25 * s;
+        y = (R.get(0, 1) + R.get(1, 0)) / s;
+        z = (R.get(0, 2) + R.get(2, 0)) / s;
+      } else if (R.get(1, 1) > R.get(2, 2)) {
+        double s = 2.0 * Math.sqrt(1.0 + R.get(1, 1) - R.get(0, 0) - R.get(2, 2));
+        w = (R.get(0, 2) - R.get(2, 0)) / s;
+        x = (R.get(0, 1) + R.get(1, 0)) / s;
+        y = 0.25 * s;
+        z = (R.get(1, 2) + R.get(2, 1)) / s;
+      } else {
+        double s = 2.0 * Math.sqrt(1.0 + R.get(2, 2) - R.get(0, 0) - R.get(1, 1));
+        w = (R.get(1, 0) - R.get(0, 1)) / s;
+        x = (R.get(0, 2) + R.get(2, 0)) / s;
+        y = (R.get(1, 2) + R.get(2, 1)) / s;
+        z = 0.25 * s;
+      }
+    }
+
+    m_rotation = new Rotation3d(new Quaternion(w, x, y, z));
+  }
+
+  /**
+   * Returns an instance of the NWU coordinate system.
+   *
+   * @return An instance of the NWU coordinate system.
+   */
+  @SuppressWarnings("MethodName")
+  public static CoordinateSystem NWU() {
+    return new CoordinateSystem(CoordinateAxis.N(), CoordinateAxis.W(), CoordinateAxis.U());
+  }
+
+  /**
+   * Returns an instance of the EDN coordinate system.
+   *
+   * @return An instance of the EDN coordinate system.
+   */
+  @SuppressWarnings("MethodName")
+  public static CoordinateSystem EDN() {
+    return new CoordinateSystem(CoordinateAxis.E(), CoordinateAxis.D(), CoordinateAxis.N());
+  }
+
+  /**
+   * Returns an instance of the NED coordinate system.
+   *
+   * @return An instance of the NED coordinate system.
+   */
+  @SuppressWarnings("MethodName")
+  public static CoordinateSystem NED() {
+    return new CoordinateSystem(CoordinateAxis.N(), CoordinateAxis.E(), CoordinateAxis.D());
+  }
+
+  /**
+   * Converts the given pose from one coordinate system to another.
+   *
+   * @param pose The pose to convert.
+   * @param from The coordinate system the pose starts in.
+   * @param to The coordinate system to which to convert.
+   * @return The given pose in the desired coordinate system.
+   */
+  public static Pose3d convert(Pose3d pose, CoordinateSystem from, CoordinateSystem to) {
+    return pose.relativeTo(new Pose3d(new Translation3d(), to.m_rotation.minus(from.m_rotation)));
+  }
+}