[wpimath] Add 3D geometry classes (#4175)

Also clean up 2D geometry documentation.

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

@@ -11,10 +11,10 @@
 using namespace frc;
 
 Pose2d::Pose2d(Translation2d translation, Rotation2d rotation)
-    : m_translation(translation), m_rotation(rotation) {}
+    : m_translation(std::move(translation)), m_rotation(std::move(rotation)) {}
 
 Pose2d::Pose2d(units::meter_t x, units::meter_t y, Rotation2d rotation)
-    : m_translation(x, y), m_rotation(rotation) {}
+    : m_translation(x, y), m_rotation(std::move(rotation)) {}
 
 Pose2d Pose2d::operator+(const Transform2d& other) const {
   return TransformBy(other);

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

@@ -0,0 +1,139 @@
+// 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/Pose3d.h"
+
+#include <cmath>
+
+using namespace frc;
+
+namespace {
+
+/**
+ * Applies the hat operator to a rotation vector.
+ *
+ * It takes a rotation vector and returns the corresponding matrix
+ * representation of the Lie algebra element (a 3x3 rotation matrix).
+ *
+ * @param rotation The rotation vector.
+ * @return The rotation vector as a 3x3 rotation matrix.
+ */
+Matrixd<3, 3> RotationVectorToMatrix(const Vectord<3>& rotation) {
+  // Given a rotation vector <a, b, c>,
+  //         [ 0 -c  b]
+  // Omega = [ c  0 -a]
+  //         [-b  a  0]
+  return Matrixd<3, 3>{{0.0, -rotation(2), rotation(1)},
+                       {rotation(2), 0.0, -rotation(0)},
+                       {-rotation(1), rotation(0), 0.0}};
+}
+}  // namespace
+
+Pose3d::Pose3d(Translation3d translation, Rotation3d rotation)
+    : m_translation(std::move(translation)), m_rotation(std::move(rotation)) {}
+
+Pose3d::Pose3d(units::meter_t x, units::meter_t y, units::meter_t z,
+               Rotation3d rotation)
+    : m_translation(x, y, z), m_rotation(std::move(rotation)) {}
+
+Pose3d Pose3d::operator+(const Transform3d& other) const {
+  return TransformBy(other);
+}
+
+Transform3d Pose3d::operator-(const Pose3d& other) const {
+  const auto pose = this->RelativeTo(other);
+  return Transform3d(pose.Translation(), pose.Rotation());
+}
+
+bool Pose3d::operator==(const Pose3d& other) const {
+  return m_translation == other.m_translation && m_rotation == other.m_rotation;
+}
+
+bool Pose3d::operator!=(const Pose3d& other) const {
+  return !operator==(other);
+}
+
+Pose3d Pose3d::TransformBy(const Transform3d& other) const {
+  return {m_translation + (other.Translation().RotateBy(m_rotation)),
+          m_rotation + other.Rotation()};
+}
+
+Pose3d Pose3d::RelativeTo(const Pose3d& other) const {
+  const Transform3d transform{other, *this};
+  return {transform.Translation(), transform.Rotation()};
+}
+
+Pose3d Pose3d::Exp(const Twist3d& twist) const {
+  Matrixd<3, 3> Omega = RotationVectorToMatrix(
+      Vectord<3>{twist.rx.value(), twist.ry.value(), twist.rz.value()});
+  Matrixd<3, 3> OmegaSq = Omega * Omega;
+
+  double thetaSq =
+      (twist.rx * twist.rx + twist.ry * twist.ry + twist.rz * twist.rz).value();
+
+  // Get left Jacobian of SO3. See first line in right column of
+  // http://asrl.utias.utoronto.ca/~tdb/bib/barfoot_ser17_identities.pdf
+  Matrixd<3, 3> J;
+  if (thetaSq < 1E-9 * 1E-9) {
+    // V = I + 0.5ω
+    J = Matrixd<3, 3>::Identity() + 0.5 * Omega;
+  } else {
+    double theta = std::sqrt(thetaSq);
+    // J = I + (1 − std::cos(θ))/θ² ω + (θ − std::sin(θ))/θ³ ω²
+    J = Matrixd<3, 3>::Identity() + (1.0 - std::cos(theta)) / thetaSq * Omega +
+        (theta - std::sin(theta)) / (thetaSq * theta) * OmegaSq;
+  }
+
+  // Get translation component
+  Vectord<3> translation =
+      J * Vectord<3>{twist.dx.value(), twist.dy.value(), twist.dz.value()};
+
+  const Transform3d transform{Translation3d{units::meter_t{translation(0)},
+                                            units::meter_t{translation(1)},
+                                            units::meter_t{translation(2)}},
+                              Rotation3d{twist.rx, twist.ry, twist.rz}};
+
+  return *this + transform;
+}
+
+Twist3d Pose3d::Log(const Pose3d& end) const {
+  const auto transform = end.RelativeTo(*this);
+
+  Vectord<3> rotVec = transform.Rotation().GetQuaternion().ToRotationVector();
+
+  Matrixd<3, 3> Omega = RotationVectorToMatrix(rotVec);
+  Matrixd<3, 3> OmegaSq = Omega * Omega;
+
+  double thetaSq = rotVec.squaredNorm();
+
+  // Get left Jacobian inverse of SO3. See fourth line in right column of
+  // http://asrl.utias.utoronto.ca/~tdb/bib/barfoot_ser17_identities.pdf
+  Matrixd<3, 3> Jinv;
+  if (thetaSq < 1E-9 * 1E-9) {
+    // J⁻¹ = I − 0.5ω + 1/12 ω²
+    Jinv = Matrixd<3, 3>::Identity() - 0.5 * Omega + 1.0 / 12.0 * OmegaSq;
+  } else {
+    double theta = std::sqrt(thetaSq);
+    double halfTheta = 0.5 * theta;
+
+    // J⁻¹ = I − 0.5ω + (1 − 0.5θ std::cos(θ/2) / std::sin(θ/2))/θ² ω²
+    Jinv = Matrixd<3, 3>::Identity() - 0.5 * Omega +
+           (1.0 - 0.5 * theta * std::cos(halfTheta) / std::sin(halfTheta)) /
+               thetaSq * OmegaSq;
+  }
+
+  // Get dtranslation component
+  Vectord<3> dtranslation =
+      Jinv * Vectord<3>{transform.X().value(), transform.Y().value(),
+                        transform.Z().value()};
+
+  return Twist3d{
+      units::meter_t{dtranslation(0)}, units::meter_t{dtranslation(1)},
+      units::meter_t{dtranslation(2)}, units::radian_t{rotVec(0)},
+      units::radian_t{rotVec(1)},      units::radian_t{rotVec(2)}};
+}
+
+Pose2d Pose3d::ToPose2d() const {
+  return Pose2d{m_translation.X(), m_translation.Y(), m_rotation.Z()};
+}

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

@@ -0,0 +1,80 @@
+// 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/Quaternion.h"
+
+using namespace frc;
+
+Quaternion::Quaternion(double w, double x, double y, double z)
+    : m_r{w}, m_v{x, y, z} {}
+
+Quaternion Quaternion::operator*(const Quaternion& other) const {
+  // https://en.wikipedia.org/wiki/Quaternion#Scalar_and_vector_parts
+  const auto& r1 = m_r;
+  const auto& v1 = m_v;
+  const auto& r2 = other.m_r;
+  const auto& v2 = other.m_v;
+
+  Eigen::Vector3d cross{v1(1) * v2(2) - v2(1) * v1(2),
+                        v2(0) * v1(2) - v1(0) * v2(2),
+                        v1(0) * v2(1) - v2(0) * v1(1)};
+
+  Eigen::Vector3d v = r1 * v2 + r2 * v1 + cross;
+  return Quaternion{r1 * r2 - v1.dot(v2), v(0), v(1), v(2)};
+}
+
+bool Quaternion::operator==(const Quaternion& other) const {
+  return std::abs(m_r * other.m_r + m_v.dot(other.m_v)) > 1.0 - 1E-9;
+}
+
+bool Quaternion::operator!=(const Quaternion& other) const {
+  return !operator==(other);
+}
+
+Quaternion Quaternion::Inverse() const {
+  return Quaternion{m_r, -m_v(0), -m_v(1), -m_v(2)};
+}
+
+Quaternion Quaternion::Normalize() const {
+  double norm = std::sqrt(W() * W() + X() * X() + Y() * Y() + Z() * Z());
+  if (norm == 0.0) {
+    return Quaternion{};
+  } else {
+    return Quaternion{W() / norm, X() / norm, Y() / norm, Z() / norm};
+  }
+}
+
+double Quaternion::W() const {
+  return m_r;
+}
+
+double Quaternion::X() const {
+  return m_v(0);
+}
+
+double Quaternion::Y() const {
+  return m_v(1);
+}
+
+double Quaternion::Z() const {
+  return m_v(2);
+}
+
+Eigen::Vector3d Quaternion::ToRotationVector() const {
+  // See equation (31) in "Integrating Generic Sensor Fusion Algorithms with
+  // Sound State Representation through Encapsulation of Manifolds"
+  //
+  // https://arxiv.org/pdf/1107.1119.pdf
+  double norm = m_v.norm();
+
+  if (norm < 1e-9) {
+    return (2.0 / W() - 2.0 / 3.0 * norm * norm / (W() * W() * W())) * m_v;
+  } else {
+    if (W() < 0.0) {
+      return 2.0 * std::atan2(-norm, -W()) / norm * m_v;
+    } else {
+      return 2.0 * std::atan2(norm, W()) / norm * m_v;
+    }
+  }
+}

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

@@ -0,0 +1,138 @@
+// 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/Rotation3d.h"
+
+#include <cmath>
+
+#include <wpi/numbers>
+
+#include "units/math.h"
+
+using namespace frc;
+
+Rotation3d::Rotation3d(const Quaternion& q) {
+  m_q = q.Normalize();
+}
+
+Rotation3d::Rotation3d(units::radian_t roll, units::radian_t pitch,
+                       units::radian_t yaw) {
+  // https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Euler_angles_to_quaternion_conversion
+  double cr = units::math::cos(roll * 0.5);
+  double sr = units::math::sin(roll * 0.5);
+
+  double cp = units::math::cos(pitch * 0.5);
+  double sp = units::math::sin(pitch * 0.5);
+
+  double cy = units::math::cos(yaw * 0.5);
+  double sy = units::math::sin(yaw * 0.5);
+
+  m_q = Quaternion{cr * cp * cy + sr * sp * sy, sr * cp * cy - cr * sp * sy,
+                   cr * sp * cy + sr * cp * sy, cr * cp * sy - sr * sp * cy};
+}
+
+Rotation3d::Rotation3d(const Vectord<3>& axis, units::radian_t angle) {
+  double norm = axis.norm();
+  if (norm == 0.0) {
+    return;
+  }
+
+  // https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Definition
+  Vectord<3> v = axis / norm * units::math::sin(angle / 2.0);
+  m_q = Quaternion{units::math::cos(angle / 2.0), v(0), v(1), v(2)};
+}
+
+Rotation3d Rotation3d::operator+(const Rotation3d& other) const {
+  return RotateBy(other);
+}
+
+Rotation3d Rotation3d::operator-(const Rotation3d& other) const {
+  return *this + -other;
+}
+
+Rotation3d Rotation3d::operator-() const {
+  return Rotation3d{m_q.Inverse()};
+}
+
+Rotation3d Rotation3d::operator*(double scalar) const {
+  // https://en.wikipedia.org/wiki/Slerp#Quaternion_Slerp
+  if (m_q.W() >= 0.0) {
+    return Rotation3d{{m_q.X(), m_q.Y(), m_q.Z()},
+                      2.0 * units::radian_t{scalar * std::acos(m_q.W())}};
+  } else {
+    return Rotation3d{{-m_q.X(), -m_q.Y(), -m_q.Z()},
+                      2.0 * units::radian_t{scalar * std::acos(-m_q.W())}};
+  }
+}
+
+bool Rotation3d::operator==(const Rotation3d& other) const {
+  return m_q == other.m_q;
+}
+
+bool Rotation3d::operator!=(const Rotation3d& other) const {
+  return !operator==(other);
+}
+
+Rotation3d Rotation3d::RotateBy(const Rotation3d& other) const {
+  return Rotation3d{other.m_q * m_q};
+}
+
+const Quaternion& Rotation3d::GetQuaternion() const {
+  return m_q;
+}
+
+units::radian_t Rotation3d::X() const {
+  double w = m_q.W();
+  double x = m_q.X();
+  double y = m_q.Y();
+  double z = m_q.Z();
+
+  // https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Quaternion_to_Euler_angles_conversion
+  return units::radian_t{
+      std::atan2(2.0 * (w * x + y * z), 1.0 - 2.0 * (x * x + y * y))};
+}
+
+units::radian_t Rotation3d::Y() const {
+  double w = m_q.W();
+  double x = m_q.X();
+  double y = m_q.Y();
+  double z = m_q.Z();
+
+  // https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Quaternion_to_Euler_angles_conversion
+  double ratio = 2.0 * (w * y - z * x);
+  if (std::abs(ratio) >= 1.0) {
+    return units::radian_t{std::copysign(wpi::numbers::pi / 2.0, ratio)};
+  } else {
+    return units::radian_t{std::asin(ratio)};
+  }
+}
+
+units::radian_t Rotation3d::Z() const {
+  double w = m_q.W();
+  double x = m_q.X();
+  double y = m_q.Y();
+  double z = m_q.Z();
+
+  // https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Quaternion_to_Euler_angles_conversion
+  return units::radian_t{
+      std::atan2(2.0 * (w * z + x * y), 1.0 - 2.0 * (y * y + z * z))};
+}
+
+Vectord<3> Rotation3d::Axis() const {
+  double norm = std::hypot(m_q.X(), m_q.Y(), m_q.Z());
+  if (norm == 0.0) {
+    return {0.0, 0.0, 0.0};
+  } else {
+    return {m_q.X() / norm, m_q.Y() / norm, m_q.Z() / norm};
+  }
+}
+
+units::radian_t Rotation3d::Angle() const {
+  double norm = std::hypot(m_q.X(), m_q.Y(), m_q.Z());
+  return units::radian_t{2.0 * std::atan2(norm, m_q.W())};
+}
+
+Rotation2d Rotation3d::ToRotation2d() const {
+  return Rotation2d{Z()};
+}

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

@@ -19,7 +19,7 @@ Transform2d::Transform2d(Pose2d initial, Pose2d final) {
 }
 
 Transform2d::Transform2d(Translation2d translation, Rotation2d rotation)
-    : m_translation(translation), m_rotation(rotation) {}
+    : m_translation(std::move(translation)), m_rotation(std::move(rotation)) {}
 
 Transform2d Transform2d::Inverse() const {
   // We are rotating the difference between the translations

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

@@ -0,0 +1,41 @@
+// 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/Transform3d.h"
+
+#include "frc/geometry/Pose3d.h"
+
+using namespace frc;
+
+Transform3d::Transform3d(Pose3d initial, Pose3d final) {
+  // We are rotating the difference between the translations
+  // using a clockwise rotation matrix. This transforms the global
+  // delta into a local delta (relative to the initial pose).
+  m_translation = (final.Translation() - initial.Translation())
+                      .RotateBy(-initial.Rotation());
+
+  m_rotation = final.Rotation() - initial.Rotation();
+}
+
+Transform3d::Transform3d(Translation3d translation, Rotation3d rotation)
+    : m_translation(std::move(translation)), m_rotation(std::move(rotation)) {}
+
+Transform3d Transform3d::Inverse() const {
+  // We are rotating the difference between the translations
+  // using a clockwise rotation matrix. This transforms the global
+  // delta into a local delta (relative to the initial pose).
+  return Transform3d{(-Translation()).RotateBy(-Rotation()), -Rotation()};
+}
+
+Transform3d Transform3d::operator+(const Transform3d& other) const {
+  return Transform3d{Pose3d{}, Pose3d{}.TransformBy(*this).TransformBy(other)};
+}
+
+bool Transform3d::operator==(const Transform3d& other) const {
+  return m_translation == other.m_translation && m_rotation == other.m_rotation;
+}
+
+bool Transform3d::operator!=(const Transform3d& other) const {
+  return !operator==(other);
+}

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

@@ -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.
+
+#include "frc/geometry/Translation3d.h"
+
+#include "units/math.h"
+
+using namespace frc;
+
+Translation3d::Translation3d(units::meter_t x, units::meter_t y,
+                             units::meter_t z)
+    : m_x(x), m_y(y), m_z(z) {}
+
+Translation3d::Translation3d(units::meter_t distance, const Rotation3d& angle) {
+  auto rectangular = Translation3d{distance, 0_m, 0_m}.RotateBy(angle);
+  m_x = rectangular.X();
+  m_y = rectangular.Y();
+  m_z = rectangular.Z();
+}
+
+units::meter_t Translation3d::Distance(const Translation3d& other) const {
+  return units::math::sqrt(units::math::pow<2>(other.m_x - m_x) +
+                           units::math::pow<2>(other.m_y - m_y) +
+                           units::math::pow<2>(other.m_z - m_z));
+}
+
+units::meter_t Translation3d::Norm() const {
+  return units::math::sqrt(m_x * m_x + m_y * m_y + m_z * m_z);
+}
+
+Translation3d Translation3d::RotateBy(const Rotation3d& other) const {
+  Quaternion p{0.0, m_x.value(), m_y.value(), m_z.value()};
+  auto qprime = other.GetQuaternion() * p * other.GetQuaternion().Inverse();
+  return Translation3d{units::meter_t{qprime.X()}, units::meter_t{qprime.Y()},
+                       units::meter_t{qprime.Z()}};
+}
+
+Translation2d Translation3d::ToTranslation2d() const {
+  return Translation2d{m_x, m_y};
+}
+
+Translation3d Translation3d::operator+(const Translation3d& other) const {
+  return {X() + other.X(), Y() + other.Y(), Z() + other.Z()};
+}
+
+Translation3d Translation3d::operator-(const Translation3d& other) const {
+  return *this + -other;
+}
+
+Translation3d Translation3d::operator-() const {
+  return {-m_x, -m_y, -m_z};
+}
+
+Translation3d Translation3d::operator*(double scalar) const {
+  return {scalar * m_x, scalar * m_y, scalar * m_z};
+}
+
+Translation3d Translation3d::operator/(double scalar) const {
+  return *this * (1.0 / scalar);
+}
+
+bool Translation3d::operator==(const Translation3d& other) const {
+  return units::math::abs(m_x - other.m_x) < 1E-9_m &&
+         units::math::abs(m_y - other.m_y) < 1E-9_m &&
+         units::math::abs(m_z - other.m_z) < 1E-9_m;
+}
+
+bool Translation3d::operator!=(const Translation3d& other) const {
+  return !operator==(other);
+}