[wpimath] Add vector projection and geometry vector conversions (#6343)

wpimath/src/main/java/edu/wpi/first/math/Vector.java

@@ -5,6 +5,7 @@
 package edu.wpi.first.math;
 
 import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.numbers.N3;
 import java.util.Objects;
 import org.ejml.simple.SimpleMatrix;
 
@@ -48,6 +49,16 @@ public class Vector<R extends Num> extends Matrix<R, N1> {
     super(other);
   }
 
+  /**
+   * Returns an element of the vector at a specified row.
+   *
+   * @param row The row that the element is located at.
+   * @return An element of the vector.
+   */
+  public double get(int row) {
+    return get(row, 0);
+  }
+
   @Override
   public Vector<R> times(double value) {
     return new Vector<>(this.m_storage.scale(value));
@@ -105,12 +116,39 @@ public class Vector<R extends Num> extends Matrix<R, N1> {
    * @return The norm.
    */
   public double norm() {
-    double squaredNorm = 0.0;
+    return Math.sqrt(dot(this));
+  }
 
-    for (int i = 0; i < getNumRows(); ++i) {
-      squaredNorm += get(i, 0) * get(i, 0);
-    }
+  /**
+   * Returns the unit vector parallel with this vector.
+   *
+   * @return The unit vector.
+   */
+  public Vector<R> unit() {
+    return div(norm());
+  }
 
-    return Math.sqrt(squaredNorm);
+  /**
+   * Returns the projection of this vector along another.
+   *
+   * @param other The vector to project along.
+   * @return The projection.
+   */
+  public Vector<R> projection(Vector<R> other) {
+    return other.times(dot(other)).div(other.dot(other));
+  }
+
+  /**
+   * Returns the cross product of 3 dimensional vectors a and b.
+   *
+   * @param a The vector to cross with b.
+   * @param b The vector to cross with a.
+   * @return The cross product of a and b.
+   */
+  public static Vector<N3> cross(Vector<N3> a, Vector<N3> b) {
+    return VecBuilder.fill(
+        a.get(1) * b.get(2) - a.get(2) * b.get(1),
+        a.get(2) * b.get(0) - a.get(0) * b.get(2),
+        a.get(0) * b.get(1) - a.get(1) * b.get(0));
   }
 }

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

@@ -11,9 +11,12 @@ import com.fasterxml.jackson.annotation.JsonCreator;
 import com.fasterxml.jackson.annotation.JsonIgnoreProperties;
 import com.fasterxml.jackson.annotation.JsonProperty;
 import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.Vector;
 import edu.wpi.first.math.geometry.proto.Translation2dProto;
 import edu.wpi.first.math.geometry.struct.Translation2dStruct;
 import edu.wpi.first.math.interpolation.Interpolatable;
+import edu.wpi.first.math.numbers.N2;
 import edu.wpi.first.units.Distance;
 import edu.wpi.first.units.Measure;
 import edu.wpi.first.util.protobuf.ProtobufSerializable;
@@ -78,6 +81,16 @@ public class Translation2d
     this(x.in(Meters), y.in(Meters));
   }
 
+  /**
+   * Constructs a Translation2d from the provided translation vector's X and Y components. The
+   * values are assumed to be in meters.
+   *
+   * @param vector The translation vector to represent.
+   */
+  public Translation2d(Vector<N2> vector) {
+    this(vector.get(0), vector.get(1));
+  }
+
   /**
    * Calculates the distance between two translations in 2D space.
    *
@@ -110,6 +123,15 @@ public class Translation2d
     return m_y;
   }
 
+  /**
+   * Returns a vector representation of this translation.
+   *
+   * @return A Vector representation of this translation.
+   */
+  public Vector<N2> toVector() {
+    return VecBuilder.fill(m_x, m_y);
+  }
+
   /**
    * Returns the norm, or distance from the origin to the translation.
    *

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

@@ -11,9 +11,12 @@ import com.fasterxml.jackson.annotation.JsonCreator;
 import com.fasterxml.jackson.annotation.JsonIgnoreProperties;
 import com.fasterxml.jackson.annotation.JsonProperty;
 import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.Vector;
 import edu.wpi.first.math.geometry.proto.Translation3dProto;
 import edu.wpi.first.math.geometry.struct.Translation3dStruct;
 import edu.wpi.first.math.interpolation.Interpolatable;
+import edu.wpi.first.math.numbers.N3;
 import edu.wpi.first.units.Distance;
 import edu.wpi.first.units.Measure;
 import edu.wpi.first.util.protobuf.ProtobufSerializable;
@@ -83,6 +86,16 @@ public class Translation3d
     this(x.in(Meters), y.in(Meters), z.in(Meters));
   }
 
+  /**
+   * Constructs a Translation3d from the provided translation vector's X, Y, and Z components. The
+   * values are assumed to be in meters.
+   *
+   * @param vector The translation vector to represent.
+   */
+  public Translation3d(Vector<N3> vector) {
+    this(vector.get(0), vector.get(1), vector.get(2));
+  }
+
   /**
    * Calculates the distance between two translations in 3D space.
    *
@@ -126,6 +139,15 @@ public class Translation3d
     return m_z;
   }
 
+  /**
+   * Returns a vector representation of this translation.
+   *
+   * @return A Vector representation of this translation.
+   */
+  public Vector<N3> toVector() {
+    return VecBuilder.fill(m_x, m_y, m_z);
+  }
+
   /**
    * Returns the norm, or distance from the origin to the translation.
    *

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

@@ -36,15 +36,15 @@ Eigen::Matrix3d RotationVectorToMatrix(const Eigen::Vector3d& rotation) {
 }  // namespace
 
 Pose3d::Pose3d(Translation3d translation, Rotation3d rotation)
-    : m_translation(std::move(translation)), m_rotation(std::move(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)) {}
+    : m_translation{x, y, z}, m_rotation{std::move(rotation)} {}
 
 Pose3d::Pose3d(const Pose2d& pose)
-    : m_translation(pose.X(), pose.Y(), 0_m),
-      m_rotation(0_rad, 0_rad, pose.Rotation().Radians()) {}
+    : m_translation{pose.X(), pose.Y(), 0_m},
+      m_rotation{0_rad, 0_rad, pose.Rotation().Radians()} {}
 
 Pose3d Pose3d::operator+(const Transform3d& other) const {
   return TransformBy(other);

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

@@ -19,11 +19,11 @@ Transform3d::Transform3d(Pose3d initial, Pose3d final) {
 }
 
 Transform3d::Transform3d(Translation3d translation, Rotation3d rotation)
-    : m_translation(std::move(translation)), m_rotation(std::move(rotation)) {}
+    : m_translation{std::move(translation)}, m_rotation{std::move(rotation)} {}
 
 Transform3d::Transform3d(units::meter_t x, units::meter_t y, units::meter_t z,
                          Rotation3d rotation)
-    : m_translation(x, y, z), m_rotation(std::move(rotation)) {}
+    : m_translation{x, y, z}, m_rotation{std::move(rotation)} {}
 
 Transform3d Transform3d::Inverse() const {
   // We are rotating the difference between the translations

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

@@ -11,6 +11,9 @@
 
 using namespace frc;
 
+Translation2d::Translation2d(const Eigen::Vector2d& vector)
+    : m_x{units::meter_t{vector.x()}}, m_y{units::meter_t{vector.y()}} {}
+
 units::meter_t Translation2d::Distance(const Translation2d& other) const {
   return units::math::hypot(other.m_x - m_x, other.m_y - m_y);
 }

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

@@ -19,6 +19,11 @@ Translation3d::Translation3d(units::meter_t distance, const Rotation3d& angle) {
   m_z = rectangular.Z();
 }
 
+Translation3d::Translation3d(const Eigen::Vector3d& vector)
+    : m_x{units::meter_t{vector.x()}},
+      m_y{units::meter_t{vector.y()}},
+      m_z{units::meter_t{vector.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) +

wpimath/src/main/native/include/frc/geometry/Pose2d.inc

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

wpimath/src/main/native/include/frc/geometry/Rotation2d.inc

@@ -12,9 +12,9 @@
 namespace frc {
 
 constexpr Rotation2d::Rotation2d(units::angle_unit auto value)
-    : m_value(value),
-      m_cos(gcem::cos(value.template convert<units::radian>().value())),
-      m_sin(gcem::sin(value.template convert<units::radian>().value())) {}
+    : m_value{value},
+      m_cos{gcem::cos(value.template convert<units::radian>().value())},
+      m_sin{gcem::sin(value.template convert<units::radian>().value())} {}
 
 constexpr Rotation2d::Rotation2d(double x, double y) {
   double magnitude = gcem::hypot(x, y);
@@ -33,7 +33,7 @@ constexpr Rotation2d Rotation2d::operator-() const {
 }
 
 constexpr Rotation2d Rotation2d::operator*(double scalar) const {
-  return Rotation2d(m_value * scalar);
+  return Rotation2d{m_value * scalar};
 }
 
 constexpr Rotation2d Rotation2d::operator+(const Rotation2d& other) const {

wpimath/src/main/native/include/frc/geometry/Transform2d.inc

@@ -14,11 +14,11 @@ namespace frc {
 
 constexpr Transform2d::Transform2d(Translation2d translation,
                                    Rotation2d rotation)
-    : m_translation(std::move(translation)), m_rotation(std::move(rotation)) {}
+    : m_translation{std::move(translation)}, m_rotation{std::move(rotation)} {}
 
 constexpr Transform2d::Transform2d(units::meter_t x, units::meter_t y,
                                    Rotation2d rotation)
-    : m_translation(x, y), m_rotation(std::move(rotation)) {}
+    : m_translation{x, y}, m_rotation{std::move(rotation)} {}
 
 constexpr Transform2d Transform2d::Inverse() const {
   // We are rotating the difference between the translations

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

@@ -7,6 +7,7 @@
 #include <initializer_list>
 #include <span>
 
+#include <Eigen/Core>
 #include <wpi/SymbolExports.h>
 #include <wpi/json_fwd.h>
 
@@ -48,6 +49,14 @@ class WPILIB_DLLEXPORT Translation2d {
    */
   constexpr Translation2d(units::meter_t distance, const Rotation2d& angle);
 
+  /**
+   * Constructs a Translation2d from the provided translation vector's X and Y
+   * components. The values are assumed to be in meters.
+   *
+   * @param vector The translation vector to represent.
+   */
+  explicit Translation2d(const Eigen::Vector2d& vector);
+
   /**
    * Calculates the distance between two translations in 2D space.
    *
@@ -73,6 +82,13 @@ class WPILIB_DLLEXPORT Translation2d {
    */
   constexpr units::meter_t Y() const { return m_y; }
 
+  /**
+   * Returns a vector representation of this translation.
+   *
+   * @return A Vector representation of this translation.
+   */
+  constexpr Eigen::Vector2d ToVector() const;
+
   /**
    * Returns the norm, or distance from the origin to the translation.
    *

wpimath/src/main/native/include/frc/geometry/Translation2d.inc

@@ -10,11 +10,15 @@
 namespace frc {
 
 constexpr Translation2d::Translation2d(units::meter_t x, units::meter_t y)
-    : m_x(x), m_y(y) {}
+    : m_x{x}, m_y{y} {}
 
 constexpr Translation2d::Translation2d(units::meter_t distance,
                                        const Rotation2d& angle)
-    : m_x(distance * angle.Cos()), m_y(distance * angle.Sin()) {}
+    : m_x{distance * angle.Cos()}, m_y{distance * angle.Sin()} {}
+
+constexpr Eigen::Vector2d Translation2d::ToVector() const {
+  return Eigen::Vector2d{{m_x.value(), m_y.value()}};
+}
 
 constexpr Rotation2d Translation2d::Angle() const {
   return Rotation2d{m_x.value(), m_y.value()};

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

@@ -4,6 +4,7 @@
 
 #pragma once
 
+#include <Eigen/Core>
 #include <wpi/SymbolExports.h>
 #include <wpi/json_fwd.h>
 
@@ -47,6 +48,14 @@ class WPILIB_DLLEXPORT Translation3d {
    */
   Translation3d(units::meter_t distance, const Rotation3d& angle);
 
+  /**
+   * Constructs a Translation3d from the provided translation vector's X, Y, and
+   * Z components. The values are assumed to be in meters.
+   *
+   * @param vector The translation vector to represent.
+   */
+  explicit Translation3d(const Eigen::Vector3d& vector);
+
   /**
    * Calculates the distance between two translations in 3D space.
    *
@@ -80,6 +89,13 @@ class WPILIB_DLLEXPORT Translation3d {
    */
   constexpr units::meter_t Z() const { return m_z; }
 
+  /**
+   * Returns a vector representation of this translation.
+   *
+   * @return A Vector representation of this translation.
+   */
+  constexpr Eigen::Vector3d ToVector() const;
+
   /**
    * Returns the norm, or distance from the origin to the translation.
    *

wpimath/src/main/native/include/frc/geometry/Translation3d.inc

@@ -13,12 +13,16 @@ namespace frc {
 
 constexpr Translation3d::Translation3d(units::meter_t x, units::meter_t y,
                                        units::meter_t z)
-    : m_x(x), m_y(y), m_z(z) {}
+    : m_x{x}, m_y{y}, m_z{z} {}
 
 constexpr Translation2d Translation3d::ToTranslation2d() const {
   return Translation2d{m_x, m_y};
 }
 
+constexpr Eigen::Vector3d Translation3d::ToVector() const {
+  return Eigen::Vector3d{{m_x.value(), m_y.value(), m_z.value()}};
+}
+
 constexpr Translation3d Translation3d::operator+(
     const Translation3d& other) const {
   return {X() + other.X(), Y() + other.Y(), Z() + other.Z()};