[wpimath] Add vector projection and geometry vector conversions

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

@@ -48,6 +48,16 @@ public Vector(Matrix<R, N1> other) {
     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 +115,25 @@ public double dot(Vector<R> other) {
    * @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> proj(Vector<R> other) {
+    return other.times(dot(other)).div(other.dot(other));
   }
 }

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

@@ -11,9 +11,12 @@
 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 Translation2d(Measure<Distance> x, Measure<Distance> y) {
     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 double getY() {
     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.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 Translation3d(Measure<Distance> x, Measure<Distance> y, Measure<Distance>
     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 double getZ() {
     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/Translation2d.cpp

@@ -11,10 +11,17 @@
 
 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);
 }
 
+Eigen::Vector2d Translation2d::ToVector() const {
+  return Eigen::Vector2d{m_x.value(), m_y.value()};
+}
+
 units::meter_t Translation2d::Norm() const {
   return units::math::hypot(m_x, m_y);
 }

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

@@ -19,12 +19,21 @@ 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) +
                            units::math::pow<2>(other.m_z - m_z));
 }
 
+Eigen::Vector3d Translation3d::ToVector() const {
+  return Eigen::Vector3d{m_x.value(), m_y.value(), m_z.value()};
+}
+
 units::meter_t Translation3d::Norm() const {
   return units::math::sqrt(m_x * m_x + m_y * m_y + m_z * m_z);
 }

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.
+   */
+  Eigen::Vector2d ToVector() const;
+
   /**
    * Returns the norm, or distance from the origin to the translation.
    *

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.
+   */
+  Eigen::Vector3d ToVector() const;
+
   /**
    * Returns the norm, or distance from the origin to the translation.
    *

wpimath/src/test/java/edu/wpi/first/math/VectorTest.java

@@ -65,4 +65,29 @@ void testVectorNorm() {
     assertEquals(Math.sqrt(14.0), VecBuilder.fill(1.0, 2.0, 3.0).norm());
     assertEquals(Math.sqrt(14.0), VecBuilder.fill(1.0, -2.0, 3.0).norm());
   }
+
+  @Test
+  void testVectorUnit() {
+    assertEquals(VecBuilder.fill(3.0, 4.0).unit(), VecBuilder.fill(3.0 / 5.0, 4.0 / 5.0));
+    assertEquals(VecBuilder.fill(8.0, 15.0).unit(), VecBuilder.fill(8.0 / 17.0, 15.0 / 17.0));
+  }
+
+  @Test
+  void testVectorProj() {
+    var vec1 = VecBuilder.fill(1.0, 2.0, 3.0);
+    var vec2 = VecBuilder.fill(4.0, 5.0, 6.0);
+    var res1 = vec1.proj(vec2);
+
+    assertEquals(res1.get(0), 1.662, 0.01);
+    assertEquals(res1.get(1), 2.077, 0.01);
+    assertEquals(res1.get(2), 2.49, 0.01);
+
+    var vec3 = VecBuilder.fill(-1.0, 2.0, -3.0);
+    var vec4 = VecBuilder.fill(4.0, -5.0, 6.0);
+    var res2 = vec4.proj(vec3);
+
+    assertEquals(res2.get(0), 2.29, 0.01);
+    assertEquals(res2.get(1), -4.57, 0.01);
+    assertEquals(res2.get(2), 6.86, 0.01);
+  }
 }

wpimath/src/test/java/edu/wpi/first/math/geometry/Translation2dTest.java

@@ -8,6 +8,7 @@
 import static org.junit.jupiter.api.Assertions.assertEquals;
 import static org.junit.jupiter.api.Assertions.assertNotEquals;
 
+import edu.wpi.first.math.VecBuilder;
 import java.util.List;
 import org.junit.jupiter.api.Test;
 
@@ -131,4 +132,15 @@ void testNearest() {
     assertEquals(origin.nearest(List.of(translation1, translation2, translation3)), translation1);
     assertEquals(origin.nearest(List.of(translation4, translation2, translation3)), translation2);
   }
+
+  @Test
+  void testVector() {
+    var vec = VecBuilder.fill(1.0, 2.0);
+    var translation = new Translation2d(vec);
+
+    assertEquals(vec.get(0), translation.getX());
+    assertEquals(vec.get(1), translation.getY());
+
+    assertEquals(vec, translation.toVector());
+  }
 }

wpimath/src/test/java/edu/wpi/first/math/geometry/Translation3dTest.java

@@ -160,4 +160,16 @@ void testPolarConstructor() {
         () -> assertEquals(Math.sqrt(3.0), two.getY(), kEpsilon),
         () -> assertEquals(0.0, two.getZ(), kEpsilon));
   }
+
+  @Test
+  void testVector() {
+    var vec = VecBuilder.fill(1.0, 2.0, 3.0);
+    var translation = new Translation3d(vec);
+
+    assertEquals(vec.get(0), translation.getX());
+    assertEquals(vec.get(1), translation.getY());
+    assertEquals(vec.get(2), translation.getZ());
+
+    assertEquals(vec, translation.toVector());
+  }
 }

wpimath/src/test/native/cpp/geometry/Translation2dTest.cpp

@@ -126,6 +126,16 @@ TEST(Translation2dTest, Nearest) {
       translation2.Y().value());
 }
 
+TEST(Translation2dTest, ToVector) {
+  const Eigen::Vector2d vec(1.0, 2.0);
+  const Translation2d translation{vec};
+
+  EXPECT_DOUBLE_EQ(vec[0], translation.X().value());
+  EXPECT_DOUBLE_EQ(vec[1], translation.Y().value());
+
+  EXPECT_TRUE(vec == translation.ToVector());
+}
+
 TEST(Translation2dTest, Constexpr) {
   constexpr Translation2d defaultCtor;
   constexpr Translation2d componentCtor{1_m, 2_m};