[wpimath] Add CoordinateSystem class (#4214)

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

@@ -0,0 +1,35 @@
+// 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.
+
+#include "frc/geometry/CoordinateAxis.h"
+
+using namespace frc;
+
+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};
+}
+
+CoordinateAxis CoordinateAxis::S() {
+  return CoordinateAxis{-1.0, 0.0, 0.0};
+}
+
+CoordinateAxis CoordinateAxis::E() {
+  return CoordinateAxis{0.0, -1.0, 0.0};
+}
+
+CoordinateAxis CoordinateAxis::W() {
+  return CoordinateAxis{0.0, 1.0, 0.0};
+}
+
+CoordinateAxis CoordinateAxis::U() {
+  return CoordinateAxis{0.0, 0.0, 1.0};
+}
+
+CoordinateAxis CoordinateAxis::D() {
+  return CoordinateAxis{0.0, 0.0, -1.0};
+}

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

@@ -0,0 +1,92 @@
+// 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.
+
+#include "frc/geometry/CoordinateSystem.h"
+
+#include <cmath>
+#include <stdexcept>
+#include <utility>
+
+#include "Eigen/QR"
+
+using namespace frc;
+
+CoordinateSystem::CoordinateSystem(const CoordinateAxis& positiveX,
+                                   const CoordinateAxis& positiveY,
+                                   const 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.
+  Matrixd<3, 3> R;
+  R.block<3, 1>(0, 0) = positiveX.m_axis;
+  R.block<3, 1>(0, 1) = positiveY.m_axis;
+  R.block<3, 1>(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 * R.transpose() != Matrixd<3, 3>::Identity()) {
+    throw std::domain_error("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(0, 0) + R(1, 1) + R(2, 2);
+  double w;
+  double x;
+  double y;
+  double z;
+
+  if (trace > 0.0) {
+    double s = 0.5 / std::sqrt(trace + 1.0);
+    w = 0.25 / s;
+    x = (R(2, 1) - R(1, 2)) * s;
+    y = (R(0, 2) - R(2, 0)) * s;
+    z = (R(1, 0) - R(0, 1)) * s;
+  } else {
+    if (R(0, 0) > R(1, 1) && R(0, 0) > R(2, 2)) {
+      double s = 2.0 * std::sqrt(1.0 + R(0, 0) - R(1, 1) - R(2, 2));
+      w = (R(2, 1) - R(1, 2)) / s;
+      x = 0.25 * s;
+      y = (R(0, 1) + R(1, 0)) / s;
+      z = (R(0, 2) + R(2, 0)) / s;
+    } else if (R(1, 1) > R(2, 2)) {
+      double s = 2.0 * std::sqrt(1.0 + R(1, 1) - R(0, 0) - R(2, 2));
+      w = (R(0, 2) - R(2, 0)) / s;
+      x = (R(0, 1) + R(1, 0)) / s;
+      y = 0.25 * s;
+      z = (R(1, 2) + R(2, 1)) / s;
+    } else {
+      double s = 2.0 * std::sqrt(1.0 + R(2, 2) - R(0, 0) - R(1, 1));
+      w = (R(1, 0) - R(0, 1)) / s;
+      x = (R(0, 2) + R(2, 0)) / s;
+      y = (R(1, 2) + R(2, 1)) / s;
+      z = 0.25 * s;
+    }
+  }
+
+  m_rotation = Rotation3d{Quaternion{w, x, y, z}};
+}
+
+CoordinateSystem CoordinateSystem::NWU() {
+  return CoordinateSystem{CoordinateAxis::N(), CoordinateAxis::W(),
+                          CoordinateAxis::U()};
+}
+
+CoordinateSystem CoordinateSystem::EDN() {
+  return CoordinateSystem{CoordinateAxis::E(), CoordinateAxis::D(),
+                          CoordinateAxis::N()};
+}
+
+CoordinateSystem CoordinateSystem::NED() {
+  return CoordinateSystem{CoordinateAxis::N(), CoordinateAxis::E(),
+                          CoordinateAxis::D()};
+}
+
+Pose3d CoordinateSystem::Convert(const Pose3d& pose,
+                                 const CoordinateSystem& from,
+                                 const CoordinateSystem& to) {
+  return pose.RelativeTo(
+      Pose3d{Translation3d{}, to.m_rotation - from.m_rotation});
+}

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

@@ -0,0 +1,73 @@
+// 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.
+
+#pragma once
+
+#include <wpi/SymbolExports.h>
+
+#include "frc/EigenCore.h"
+#include "frc/geometry/Pose3d.h"
+#include "frc/geometry/Rotation3d.h"
+
+namespace frc {
+
+/**
+ * A class representing a coordinate system axis within the NWU coordinate
+ * system.
+ */
+class WPILIB_DLLEXPORT CoordinateAxis {
+ public:
+  /**
+   * 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.
+   */
+  CoordinateAxis(double x, double y, double z);
+
+  CoordinateAxis(const CoordinateAxis&) = default;
+  CoordinateAxis& operator=(const CoordinateAxis&) = default;
+
+  CoordinateAxis(CoordinateAxis&&) = default;
+  CoordinateAxis& operator=(CoordinateAxis&&) = default;
+
+  /**
+   * Returns a coordinate axis corresponding to +X in the NWU coordinate system.
+   */
+  static CoordinateAxis N();
+
+  /**
+   * Returns a coordinate axis corresponding to -X in the NWU coordinate system.
+   */
+  static CoordinateAxis S();
+
+  /**
+   * Returns a coordinate axis corresponding to -Y in the NWU coordinate system.
+   */
+  static CoordinateAxis E();
+
+  /**
+   * Returns a coordinate axis corresponding to +Y in the NWU coordinate system.
+   */
+  static CoordinateAxis W();
+
+  /**
+   * Returns a coordinate axis corresponding to +Z in the NWU coordinate system.
+   */
+  static CoordinateAxis U();
+
+  /**
+   * Returns a coordinate axis corresponding to -Z in the NWU coordinate system.
+   */
+  static CoordinateAxis D();
+
+ private:
+  friend class CoordinateSystem;
+
+  Vectord<3> m_axis;
+};
+
+}  // namespace frc

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

@@ -0,0 +1,71 @@
+// 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.
+
+#pragma once
+
+#include <wpi/SymbolExports.h>
+
+#include "frc/EigenCore.h"
+#include "frc/geometry/CoordinateAxis.h"
+#include "frc/geometry/Pose3d.h"
+#include "frc/geometry/Rotation3d.h"
+
+namespace frc {
+
+/**
+ * A helper class that converts Pose3d objects between different standard
+ * coordinate frames.
+ */
+class WPILIB_DLLEXPORT CoordinateSystem {
+ public:
+  /**
+   * 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 std::domain_error if the coordinate system isn't special orthogonal
+   */
+  CoordinateSystem(const CoordinateAxis& positiveX,
+                   const CoordinateAxis& positiveY,
+                   const CoordinateAxis& positiveZ);
+
+  CoordinateSystem(const CoordinateSystem&) = default;
+  CoordinateSystem& operator=(const CoordinateSystem&) = default;
+  CoordinateSystem(CoordinateSystem&&) = default;
+  CoordinateSystem& operator=(CoordinateSystem&&) = default;
+
+  /**
+   * Returns an instance of the NWU coordinate system.
+   */
+  static CoordinateSystem NWU();
+
+  /**
+   * Returns an instance of the EDN coordinate system.
+   */
+  static CoordinateSystem EDN();
+
+  /**
+   * Returns an instance of the NED coordinate system.
+   */
+  static CoordinateSystem NED();
+
+  /**
+   * 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.
+   */
+  static Pose3d Convert(const Pose3d& pose, const CoordinateSystem& from,
+                        const CoordinateSystem& to);
+
+ private:
+  // Rotation from this coordinate system to NWU coordinate system
+  Rotation3d m_rotation;
+};
+
+}  // namespace frc