[wpimath] Make geometry classes constexpr (#7222)

Author: calcmogul

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

@@ -295,7 +295,9 @@ public Quaternion log() {
     double v_scalar;
 
     if (v_norm < 1e-9) {
-      // Taylor series expansion of atan2(y / x) / y around y = 0 => 1/x - y²/3*x³ + O(y⁴)
+      // Taylor series expansion of atan2(y/x)/y at y = 0:
+      //
+      //   1/x - 1/3 y²/x³ + O(y⁴)
       v_scalar = 1.0 / getW() - 1.0 / 3.0 * v_norm * v_norm / (getW() * getW() * getW());
     } else {
       v_scalar = Math.atan2(v_norm, getW()) / v_norm;

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

@@ -24,7 +24,6 @@
 import edu.wpi.first.util.protobuf.ProtobufSerializable;
 import edu.wpi.first.util.struct.StructSerializable;
 import java.util.Objects;
-import org.ejml.dense.row.factory.DecompositionFactory_DDRM;
 
 /** A rotation in a 3D coordinate frame represented by a quaternion. */
 @JsonIgnoreProperties(ignoreUnknown = true)
@@ -230,27 +229,42 @@ public Rotation3d(Vector<N3> initial, Vector<N3> last) {
       // there's no rotation. The default initialization of m_q will work.
       m_q = new Quaternion();
     } else if (dotNorm < -1.0 + 1E-9) {
-      // If the dot product is -1, the two vectors point in opposite directions
-      // so a 180 degree rotation is required. Any orthogonal vector can be used
-      // for it. Q in the QR decomposition is an orthonormal basis, so it
-      // contains orthogonal unit vectors.
-      var X = VecBuilder.fill(initial.get(0, 0), initial.get(1, 0), initial.get(2, 0));
-      final var qr = DecompositionFactory_DDRM.qr(3, 1);
-      qr.decompose(X.getStorage().getMatrix());
-      final var Q = qr.getQ(null, false);
-
-      // w = cos(θ/2) = cos(90°) = 0
-      //
-      // For x, y, and z, we use the second column of Q because the first is
-      // parallel instead of orthogonal. The third column would also work.
-      m_q = new Quaternion(0.0, Q.get(0, 1), Q.get(1, 1), Q.get(2, 1));
+      // If the dot product is -1, the two vectors are antiparallel, so a 180°
+      // rotation is required. Any other vector can be used to generate an
+      // orthogonal one.
+
+      double x = Math.abs(initial.get(0, 0));
+      double y = Math.abs(initial.get(1, 0));
+      double z = Math.abs(initial.get(2, 0));
+
+      // Find vector that is most orthogonal to initial vector
+      Vector<N3> other;
+      if (x < y) {
+        if (x < z) {
+          // Use x-axis
+          other = VecBuilder.fill(1, 0, 0);
+        } else {
+          // Use z-axis
+          other = VecBuilder.fill(0, 0, 1);
+        }
+      } else {
+        if (y < z) {
+          // Use y-axis
+          other = VecBuilder.fill(0, 1, 0);
+        } else {
+          // Use z-axis
+          other = VecBuilder.fill(0, 0, 1);
+        }
+      }
+
+      var axis = Vector.cross(initial, other);
+
+      double axisNorm = axis.norm();
+      m_q =
+          new Quaternion(
+              0.0, axis.get(0, 0) / axisNorm, axis.get(1, 0) / axisNorm, axis.get(2, 0) / axisNorm);
     } else {
-      // initial x last
-      var axis =
-          VecBuilder.fill(
-              initial.get(1, 0) * last.get(2, 0) - last.get(1, 0) * initial.get(2, 0),
-              last.get(0, 0) * initial.get(2, 0) - initial.get(0, 0) * last.get(2, 0),
-              initial.get(0, 0) * last.get(1, 0) - last.get(0, 0) * initial.get(1, 0));
+      var axis = Vector.cross(initial, last);
 
       // https://stackoverflow.com/a/11741520
       m_q =

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

@@ -8,10 +8,6 @@
 
 using namespace frc;
 
-units::meter_t Ellipse2d::Distance(const Translation2d& point) const {
-  return FindNearestPoint(point).Distance(point);
-}
-
 Translation2d Ellipse2d::FindNearestPoint(const Translation2d& point) const {
   // Check if already in ellipse
   if (Contains(point)) {

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

@@ -4,101 +4,8 @@
 
 #include "frc/geometry/Pose2d.h"
 
-#include <cmath>
-
 #include <wpi/json.h>
 
-#include "frc/MathUtil.h"
-
-using namespace frc;
-
-Transform2d Pose2d::operator-(const Pose2d& other) const {
-  const auto pose = this->RelativeTo(other);
-  return Transform2d{pose.Translation(), pose.Rotation()};
-}
-
-Pose2d Pose2d::RelativeTo(const Pose2d& other) const {
-  const Transform2d transform{other, *this};
-  return {transform.Translation(), transform.Rotation()};
-}
-
-Pose2d Pose2d::Exp(const Twist2d& twist) const {
-  const auto dx = twist.dx;
-  const auto dy = twist.dy;
-  const auto dtheta = twist.dtheta.value();
-
-  const auto sinTheta = std::sin(dtheta);
-  const auto cosTheta = std::cos(dtheta);
-
-  double s, c;
-  if (std::abs(dtheta) < 1E-9) {
-    s = 1.0 - 1.0 / 6.0 * dtheta * dtheta;
-    c = 0.5 * dtheta;
-  } else {
-    s = sinTheta / dtheta;
-    c = (1 - cosTheta) / dtheta;
-  }
-
-  const Transform2d transform{Translation2d{dx * s - dy * c, dx * c + dy * s},
-                              Rotation2d{cosTheta, sinTheta}};
-
-  return *this + transform;
-}
-
-Twist2d Pose2d::Log(const Pose2d& end) const {
-  const auto transform = end.RelativeTo(*this);
-  const auto dtheta = transform.Rotation().Radians().value();
-  const auto halfDtheta = dtheta / 2.0;
-
-  const auto cosMinusOne = transform.Rotation().Cos() - 1;
-
-  double halfThetaByTanOfHalfDtheta;
-
-  if (std::abs(cosMinusOne) < 1E-9) {
-    halfThetaByTanOfHalfDtheta = 1.0 - 1.0 / 12.0 * dtheta * dtheta;
-  } else {
-    halfThetaByTanOfHalfDtheta =
-        -(halfDtheta * transform.Rotation().Sin()) / cosMinusOne;
-  }
-
-  const Translation2d translationPart =
-      transform.Translation().RotateBy(
-          {halfThetaByTanOfHalfDtheta, -halfDtheta}) *
-      std::hypot(halfThetaByTanOfHalfDtheta, halfDtheta);
-
-  return {translationPart.X(), translationPart.Y(), units::radian_t{dtheta}};
-}
-
-Pose2d Pose2d::Nearest(std::span<const Pose2d> poses) const {
-  return *std::min_element(
-      poses.begin(), poses.end(), [this](const Pose2d& a, const Pose2d& b) {
-        auto aDistance = this->Translation().Distance(a.Translation());
-        auto bDistance = this->Translation().Distance(b.Translation());
-
-        // If the distances are equal sort by difference in rotation
-        if (aDistance == bDistance) {
-          return std::abs((this->Rotation() - a.Rotation()).Radians().value()) <
-                 std::abs((this->Rotation() - b.Rotation()).Radians().value());
-        }
-        return aDistance < bDistance;
-      });
-}
-
-Pose2d Pose2d::Nearest(std::initializer_list<Pose2d> poses) const {
-  return *std::min_element(
-      poses.begin(), poses.end(), [this](const Pose2d& a, const Pose2d& b) {
-        auto aDistance = this->Translation().Distance(a.Translation());
-        auto bDistance = this->Translation().Distance(b.Translation());
-
-        // If the distances are equal sort by difference in rotation
-        if (aDistance == bDistance) {
-          return std::abs((this->Rotation() - a.Rotation()).Radians().value()) <
-                 std::abs((this->Rotation() - b.Rotation()).Radians().value());
-        }
-        return aDistance < bDistance;
-      });
-}
-
 void frc::to_json(wpi::json& json, const Pose2d& pose) {
   json = wpi::json{{"translation", pose.Translation()},
                    {"rotation", pose.Rotation()}};