From 663187f689b654703fbcb7caa17d7848582d5e78 Mon Sep 17 00:00:00 2001
From: Asa Paparo <asapaparo@gmail.com>
Date: Sun, 4 Feb 2024 15:03:54 -0500
Subject: [PATCH 01/19] add vector projection and geometry getVector methods

---
 .../main/java/edu/wpi/first/math/Vector.java  | 43 ++++++++++++++++---
 .../wpi/first/math/geometry/Rotation2d.java   | 13 ++++++
 .../first/math/geometry/Translation2d.java    | 22 ++++++++++
 .../first/math/geometry/Translation3d.java    | 22 ++++++++++
 4 files changed, 95 insertions(+), 5 deletions(-)

diff --git a/wpimath/src/main/java/edu/wpi/first/math/Vector.java b/wpimath/src/main/java/edu/wpi/first/math/Vector.java
index bc467411259..7d2adf8e988 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/Vector.java
+++ b/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,35 @@ 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());
+  }
+
+  /**
+   * Returns the scalar component of this vector along another.
+   *
+   * @param other The vector to find the component of this along.
+   * @return The component.
+   */
+  public double comp(Vector<R> other) {
+    return dot(other) / other.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.unit().times(comp(other));
   }
 }
diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation2d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation2d.java
index 1106a103a52..5ae071627b0 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation2d.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation2d.java
@@ -10,10 +10,14 @@
 import com.fasterxml.jackson.annotation.JsonCreator;
 import com.fasterxml.jackson.annotation.JsonIgnoreProperties;
 import com.fasterxml.jackson.annotation.JsonProperty;
+import edu.wpi.first.math.MatBuilder;
 import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.Nat;
 import edu.wpi.first.math.geometry.proto.Rotation2dProto;
 import edu.wpi.first.math.geometry.struct.Rotation2dStruct;
 import edu.wpi.first.math.interpolation.Interpolatable;
+import edu.wpi.first.math.numbers.N2;
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.units.Angle;
 import edu.wpi.first.units.Measure;
@@ -245,6 +249,15 @@ public double getTan() {
     return m_sin / m_cos;
   }
 
+  /**
+   * Returns the rotation matrix representation of the Rotation2d.
+   *
+   * @return A rotation matrix.
+   */
+  public Matrix<N2, N2> getRotationMatrix() {
+    return MatBuilder.fill(Nat.N2(), Nat.N2(), m_cos, -m_sin, m_sin, m_cos);
+  }
+
   @Override
   public String toString() {
     return String.format("Rotation2d(Rads: %.2f, Deg: %.2f)", m_value, Math.toDegrees(m_value));
diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation2d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation2d.java
index f089f6690f3..370729e38f4 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation2d.java
+++ b/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.
+   */
+  public Vector<N2> getVector() {
+    return VecBuilder.fill(m_x, m_y);
+  }
+
   /**
    * Returns the norm, or distance from the origin to the translation.
    *
diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation3d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation3d.java
index 6d4fa9247e3..1ffb997740b 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation3d.java
+++ b/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.
+   */
+  public Vector<N3> getVector() {
+    return VecBuilder.fill(m_x, m_y, m_z);
+  }
+
   /**
    * Returns the norm, or distance from the origin to the translation.
    *

From f6b350c2a1956757b8958cea9e895d33201a3b8f Mon Sep 17 00:00:00 2001
From: Asa Paparo <asapaparo@gmail.com>
Date: Sun, 4 Feb 2024 15:35:10 -0500
Subject: [PATCH 02/19] added tests

---
 .../java/edu/wpi/first/math/VectorTest.java   | 19 +++++++++++++++++++
 .../first/math/geometry/Rotation2dTest.java   | 13 +++++++++++++
 .../math/geometry/Translation2dTest.java      | 12 ++++++++++++
 .../math/geometry/Translation3dTest.java      | 12 ++++++++++++
 4 files changed, 56 insertions(+)

diff --git a/wpimath/src/test/java/edu/wpi/first/math/VectorTest.java b/wpimath/src/test/java/edu/wpi/first/math/VectorTest.java
index 008953f9170..0a1356985c3 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/VectorTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/VectorTest.java
@@ -65,4 +65,23 @@ 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 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);
+  }
 }
diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/Rotation2dTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/Rotation2dTest.java
index 8a5dee60ea2..f8520834c35 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/geometry/Rotation2dTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/Rotation2dTest.java
@@ -7,7 +7,11 @@
 import static org.junit.jupiter.api.Assertions.assertAll;
 import static org.junit.jupiter.api.Assertions.assertEquals;
 import static org.junit.jupiter.api.Assertions.assertNotEquals;
+import static org.junit.jupiter.api.Assertions.assertTrue;
 
+import edu.wpi.first.math.MatBuilder;
+import edu.wpi.first.math.Nat;
+import edu.wpi.first.math.VecBuilder;
 import edu.wpi.first.units.Units;
 import org.junit.jupiter.api.Test;
 
@@ -114,4 +118,13 @@ void testInterpolate() {
     interpolated = rot1.interpolate(rot2, 0.5);
     assertEquals(-175.0, interpolated.getDegrees(), kEpsilon);
   }
+
+  @Test
+  void testMatrix() {
+    var rotation = Rotation2d.fromDegrees(90);
+    var point = VecBuilder.fill(1.0, 2.0);
+    var rotated = rotation.getRotationMatrix().times(point);
+    var expected = MatBuilder.fill(Nat.N2(), Nat.N1(), -2.0, 1.0);
+    assertTrue(rotated.isEqual(expected, 0.01));
+  }
 }
diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/Translation2dTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/Translation2dTest.java
index 6ed8a61478b..76b096adc3e 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/geometry/Translation2dTest.java
+++ b/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.getVector());
+  }
 }
diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/Translation3dTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/Translation3dTest.java
index cd68dbec77d..0a5f088cc35 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/geometry/Translation3dTest.java
+++ b/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.getVector());
+  }
 }

From 9db47c99b6fda289f3af3c4a54d15ce0b05a1a51 Mon Sep 17 00:00:00 2001
From: Asa Paparo <asapaparo@gmail.com>
Date: Sun, 4 Feb 2024 15:47:35 -0500
Subject: [PATCH 03/19] removed Rotation2d.getMatrix

---
 .../java/edu/wpi/first/math/geometry/Rotation2d.java     | 9 ---------
 .../java/edu/wpi/first/math/geometry/Rotation2dTest.java | 9 ---------
 2 files changed, 18 deletions(-)

diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation2d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation2d.java
index 5ae071627b0..ccfbd66f0a9 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation2d.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation2d.java
@@ -249,15 +249,6 @@ public double getTan() {
     return m_sin / m_cos;
   }
 
-  /**
-   * Returns the rotation matrix representation of the Rotation2d.
-   *
-   * @return A rotation matrix.
-   */
-  public Matrix<N2, N2> getRotationMatrix() {
-    return MatBuilder.fill(Nat.N2(), Nat.N2(), m_cos, -m_sin, m_sin, m_cos);
-  }
-
   @Override
   public String toString() {
     return String.format("Rotation2d(Rads: %.2f, Deg: %.2f)", m_value, Math.toDegrees(m_value));
diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/Rotation2dTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/Rotation2dTest.java
index f8520834c35..4632f3040c6 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/geometry/Rotation2dTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/Rotation2dTest.java
@@ -118,13 +118,4 @@ void testInterpolate() {
     interpolated = rot1.interpolate(rot2, 0.5);
     assertEquals(-175.0, interpolated.getDegrees(), kEpsilon);
   }
-
-  @Test
-  void testMatrix() {
-    var rotation = Rotation2d.fromDegrees(90);
-    var point = VecBuilder.fill(1.0, 2.0);
-    var rotated = rotation.getRotationMatrix().times(point);
-    var expected = MatBuilder.fill(Nat.N2(), Nat.N1(), -2.0, 1.0);
-    assertTrue(rotated.isEqual(expected, 0.01));
-  }
 }

From 6f3e094665f7317733f01cd48fb22d000046d82f Mon Sep 17 00:00:00 2001
From: Asa Paparo <asapaparo@gmail.com>
Date: Sun, 4 Feb 2024 15:49:28 -0500
Subject: [PATCH 04/19] naming changes

---
 wpimath/src/main/java/edu/wpi/first/math/Vector.java        | 6 +++---
 .../main/java/edu/wpi/first/math/geometry/Rotation2d.java   | 4 ----
 .../java/edu/wpi/first/math/geometry/Translation2d.java     | 2 +-
 .../java/edu/wpi/first/math/geometry/Translation3d.java     | 2 +-
 wpimath/src/test/java/edu/wpi/first/math/VectorTest.java    | 4 ++--
 .../java/edu/wpi/first/math/geometry/Rotation2dTest.java    | 4 ----
 .../java/edu/wpi/first/math/geometry/Translation2dTest.java | 2 +-
 .../java/edu/wpi/first/math/geometry/Translation3dTest.java | 2 +-
 8 files changed, 9 insertions(+), 17 deletions(-)

diff --git a/wpimath/src/main/java/edu/wpi/first/math/Vector.java b/wpimath/src/main/java/edu/wpi/first/math/Vector.java
index 7d2adf8e988..f91158c7e10 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/Vector.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/Vector.java
@@ -133,7 +133,7 @@ public Vector<R> unit() {
    * @param other The vector to find the component of this along.
    * @return The component.
    */
-  public double comp(Vector<R> other) {
+  public double component(Vector<R> other) {
     return dot(other) / other.norm();
   }
 
@@ -143,7 +143,7 @@ public double comp(Vector<R> other) {
    * @param other The vector to project along.
    * @return The projection.
    */
-  public Vector<R> proj(Vector<R> other) {
-    return other.unit().times(comp(other));
+  public Vector<R> projection(Vector<R> other) {
+    return other.unit().times(component(other));
   }
 }
diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation2d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation2d.java
index ccfbd66f0a9..1106a103a52 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation2d.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation2d.java
@@ -10,14 +10,10 @@
 import com.fasterxml.jackson.annotation.JsonCreator;
 import com.fasterxml.jackson.annotation.JsonIgnoreProperties;
 import com.fasterxml.jackson.annotation.JsonProperty;
-import edu.wpi.first.math.MatBuilder;
 import edu.wpi.first.math.MathUtil;
-import edu.wpi.first.math.Matrix;
-import edu.wpi.first.math.Nat;
 import edu.wpi.first.math.geometry.proto.Rotation2dProto;
 import edu.wpi.first.math.geometry.struct.Rotation2dStruct;
 import edu.wpi.first.math.interpolation.Interpolatable;
-import edu.wpi.first.math.numbers.N2;
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.units.Angle;
 import edu.wpi.first.units.Measure;
diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation2d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation2d.java
index 370729e38f4..282926e8a7c 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation2d.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation2d.java
@@ -128,7 +128,7 @@ public double getY() {
    *
    * @return A Vector.
    */
-  public Vector<N2> getVector() {
+  public Vector<N2> toVector() {
     return VecBuilder.fill(m_x, m_y);
   }
 
diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation3d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation3d.java
index 1ffb997740b..2aee7f7ce4f 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation3d.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation3d.java
@@ -144,7 +144,7 @@ public double getZ() {
    *
    * @return A Vector.
    */
-  public Vector<N3> getVector() {
+  public Vector<N3> toVector() {
     return VecBuilder.fill(m_x, m_y, m_z);
   }
 
diff --git a/wpimath/src/test/java/edu/wpi/first/math/VectorTest.java b/wpimath/src/test/java/edu/wpi/first/math/VectorTest.java
index 0a1356985c3..2a51b8e07c7 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/VectorTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/VectorTest.java
@@ -70,7 +70,7 @@ void testVectorNorm() {
   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);
+    var res1 = vec1.projection(vec2);
 
     assertEquals(res1.get(0), 1.662, 0.01);
     assertEquals(res1.get(1), 2.077, 0.01);
@@ -78,7 +78,7 @@ void testVectorProj() {
 
     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);
+    var res2 = vec4.projection(vec3);
 
     assertEquals(res2.get(0), 2.29, 0.01);
     assertEquals(res2.get(1), -4.57, 0.01);
diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/Rotation2dTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/Rotation2dTest.java
index 4632f3040c6..8a5dee60ea2 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/geometry/Rotation2dTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/Rotation2dTest.java
@@ -7,11 +7,7 @@
 import static org.junit.jupiter.api.Assertions.assertAll;
 import static org.junit.jupiter.api.Assertions.assertEquals;
 import static org.junit.jupiter.api.Assertions.assertNotEquals;
-import static org.junit.jupiter.api.Assertions.assertTrue;
 
-import edu.wpi.first.math.MatBuilder;
-import edu.wpi.first.math.Nat;
-import edu.wpi.first.math.VecBuilder;
 import edu.wpi.first.units.Units;
 import org.junit.jupiter.api.Test;
 
diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/Translation2dTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/Translation2dTest.java
index 76b096adc3e..df888ff6d5a 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/geometry/Translation2dTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/Translation2dTest.java
@@ -141,6 +141,6 @@ void testVector() {
     assertEquals(vec.get(0), translation.getX());
     assertEquals(vec.get(1), translation.getY());
 
-    assertEquals(vec, translation.getVector());
+    assertEquals(vec, translation.toVector());
   }
 }
diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/Translation3dTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/Translation3dTest.java
index 0a5f088cc35..9d6137761d4 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/geometry/Translation3dTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/Translation3dTest.java
@@ -170,6 +170,6 @@ void testVector() {
     assertEquals(vec.get(1), translation.getY());
     assertEquals(vec.get(2), translation.getZ());
 
-    assertEquals(vec, translation.getVector());
+    assertEquals(vec, translation.toVector());
   }
 }

From 82b7a96db619dd01e616d1e3bd0e951fc39549b1 Mon Sep 17 00:00:00 2001
From: Asa Paparo <asapaparo@gmail.com>
Date: Sun, 4 Feb 2024 17:16:31 -0500
Subject: [PATCH 05/19] add c++ functions and tests

---
 .../wpi/first/math/geometry/Translation2d.java   |  2 +-
 .../wpi/first/math/geometry/Translation3d.java   |  2 +-
 .../main/native/cpp/geometry/Translation2d.cpp   |  4 ++++
 .../main/native/cpp/geometry/Translation3d.cpp   |  4 ++++
 .../native/include/frc/geometry/Translation2d.h  | 16 ++++++++++++++++
 .../include/frc/geometry/Translation2d.inc       |  3 +++
 .../native/include/frc/geometry/Translation3d.h  | 16 ++++++++++++++++
 .../include/frc/geometry/Translation3d.inc       |  4 ++++
 .../native/cpp/geometry/Translation2dTest.cpp    | 10 ++++++++++
 9 files changed, 59 insertions(+), 2 deletions(-)

diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation2d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation2d.java
index 282926e8a7c..4d04df354e3 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation2d.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation2d.java
@@ -126,7 +126,7 @@ public double getY() {
   /**
    * Returns a vector representation of this translation.
    *
-   * @return A Vector.
+   * @return A Vector representation of this translation.
    */
   public Vector<N2> toVector() {
     return VecBuilder.fill(m_x, m_y);
diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation3d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation3d.java
index 2aee7f7ce4f..6068979fc38 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation3d.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation3d.java
@@ -142,7 +142,7 @@ public double getZ() {
   /**
    * Returns a vector representation of this translation.
    *
-   * @return A Vector.
+   * @return A Vector representation of this translation.
    */
   public Vector<N3> toVector() {
     return VecBuilder.fill(m_x, m_y, m_z);
diff --git a/wpimath/src/main/native/cpp/geometry/Translation2d.cpp b/wpimath/src/main/native/cpp/geometry/Translation2d.cpp
index c875582820e..268c139a313 100644
--- a/wpimath/src/main/native/cpp/geometry/Translation2d.cpp
+++ b/wpimath/src/main/native/cpp/geometry/Translation2d.cpp
@@ -15,6 +15,10 @@ 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);
 }
diff --git a/wpimath/src/main/native/cpp/geometry/Translation3d.cpp b/wpimath/src/main/native/cpp/geometry/Translation3d.cpp
index ecfee1ca3a2..b422c37d0f4 100644
--- a/wpimath/src/main/native/cpp/geometry/Translation3d.cpp
+++ b/wpimath/src/main/native/cpp/geometry/Translation3d.cpp
@@ -25,6 +25,10 @@ units::meter_t Translation3d::Distance(const Translation3d& other) const {
                            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);
 }
diff --git a/wpimath/src/main/native/include/frc/geometry/Translation2d.h b/wpimath/src/main/native/include/frc/geometry/Translation2d.h
index a99492dcebd..0361eb41519 100644
--- a/wpimath/src/main/native/include/frc/geometry/Translation2d.h
+++ b/wpimath/src/main/native/include/frc/geometry/Translation2d.h
@@ -4,6 +4,7 @@
 
 #pragma once
 
+#include <Eigen/Core>
 #include <initializer_list>
 #include <span>
 
@@ -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.
+   */
+  constexpr 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.
    *
diff --git a/wpimath/src/main/native/include/frc/geometry/Translation2d.inc b/wpimath/src/main/native/include/frc/geometry/Translation2d.inc
index 3be897fad6d..297f96558cb 100644
--- a/wpimath/src/main/native/include/frc/geometry/Translation2d.inc
+++ b/wpimath/src/main/native/include/frc/geometry/Translation2d.inc
@@ -16,6 +16,9 @@ constexpr Translation2d::Translation2d(units::meter_t distance,
                                        const Rotation2d& angle)
     : m_x(distance * angle.Cos()), m_y(distance * angle.Sin()) {}
 
+constexpr Translation2d::Translation2d(const Eigen::Vector2d& vector)
+    : m_x(units::meter_t(vector.x())), m_y(units::meter_t(vector.y())) {}
+
 constexpr Rotation2d Translation2d::Angle() const {
   return Rotation2d{m_x.value(), m_y.value()};
 }
diff --git a/wpimath/src/main/native/include/frc/geometry/Translation3d.h b/wpimath/src/main/native/include/frc/geometry/Translation3d.h
index b0b3359d96e..cd0cef5266a 100644
--- a/wpimath/src/main/native/include/frc/geometry/Translation3d.h
+++ b/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.
+   */
+  constexpr 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.
    *
diff --git a/wpimath/src/main/native/include/frc/geometry/Translation3d.inc b/wpimath/src/main/native/include/frc/geometry/Translation3d.inc
index 8ab6e94c609..d3fcb7412d5 100644
--- a/wpimath/src/main/native/include/frc/geometry/Translation3d.inc
+++ b/wpimath/src/main/native/include/frc/geometry/Translation3d.inc
@@ -15,6 +15,10 @@ constexpr Translation3d::Translation3d(units::meter_t x, units::meter_t y,
                                        units::meter_t z)
     : m_x(x), m_y(y), m_z(z) {}
 
+constexpr 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())) {}
+
+
 constexpr Translation2d Translation3d::ToTranslation2d() const {
   return Translation2d{m_x, m_y};
 }
diff --git a/wpimath/src/test/native/cpp/geometry/Translation2dTest.cpp b/wpimath/src/test/native/cpp/geometry/Translation2dTest.cpp
index 1c92c0a8b8c..46d383ea584 100644
--- a/wpimath/src/test/native/cpp/geometry/Translation2dTest.cpp
+++ b/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};

From 029946c8491012d7bec17b999d65fd0365fe4399 Mon Sep 17 00:00:00 2001
From: "github-actions[bot]"
 <41898282+github-actions[bot]@users.noreply.github.com>
Date: Sun, 4 Feb 2024 22:25:13 +0000
Subject: [PATCH 06/19] Formatting fixes

---
 .../src/main/native/include/frc/geometry/Translation2d.h    | 6 +++---
 .../src/main/native/include/frc/geometry/Translation3d.h    | 4 ++--
 .../src/main/native/include/frc/geometry/Translation3d.inc  | 5 +++--
 3 files changed, 8 insertions(+), 7 deletions(-)

diff --git a/wpimath/src/main/native/include/frc/geometry/Translation2d.h b/wpimath/src/main/native/include/frc/geometry/Translation2d.h
index 0361eb41519..0e389c7effe 100644
--- a/wpimath/src/main/native/include/frc/geometry/Translation2d.h
+++ b/wpimath/src/main/native/include/frc/geometry/Translation2d.h
@@ -4,10 +4,10 @@
 
 #pragma once
 
-#include <Eigen/Core>
 #include <initializer_list>
 #include <span>
 
+#include <Eigen/Core>
 #include <wpi/SymbolExports.h>
 #include <wpi/json_fwd.h>
 
@@ -50,8 +50,8 @@ 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.
+   * 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.
    */
diff --git a/wpimath/src/main/native/include/frc/geometry/Translation3d.h b/wpimath/src/main/native/include/frc/geometry/Translation3d.h
index cd0cef5266a..782d0a80778 100644
--- a/wpimath/src/main/native/include/frc/geometry/Translation3d.h
+++ b/wpimath/src/main/native/include/frc/geometry/Translation3d.h
@@ -49,8 +49,8 @@ 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.
+   * 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.
    */
diff --git a/wpimath/src/main/native/include/frc/geometry/Translation3d.inc b/wpimath/src/main/native/include/frc/geometry/Translation3d.inc
index d3fcb7412d5..5b17af597e7 100644
--- a/wpimath/src/main/native/include/frc/geometry/Translation3d.inc
+++ b/wpimath/src/main/native/include/frc/geometry/Translation3d.inc
@@ -16,8 +16,9 @@ constexpr Translation3d::Translation3d(units::meter_t x, units::meter_t y,
     : m_x(x), m_y(y), m_z(z) {}
 
 constexpr 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())) {}
-
+    : m_x(units::meter_t(vector.x())),
+      m_y(units::meter_t(vector.y())),
+      m_z(units::meter_t(vector.z())) {}
 
 constexpr Translation2d Translation3d::ToTranslation2d() const {
   return Translation2d{m_x, m_y};

From d6d9ca10735bfd85a8a4f77e40e07d7968a35f3f Mon Sep 17 00:00:00 2001
From: Asa Paparo <asapaparo@gmail.com>
Date: Sun, 4 Feb 2024 17:36:57 -0500
Subject: [PATCH 07/19] made vector ctors not constexpr

---
 wpimath/src/main/native/cpp/geometry/Translation2d.cpp       | 3 +++
 wpimath/src/main/native/cpp/geometry/Translation3d.cpp       | 5 +++++
 wpimath/src/main/native/include/frc/geometry/Translation2d.h | 2 +-
 .../src/main/native/include/frc/geometry/Translation2d.inc   | 3 ---
 wpimath/src/main/native/include/frc/geometry/Translation3d.h | 2 +-
 .../src/main/native/include/frc/geometry/Translation3d.inc   | 5 -----
 6 files changed, 10 insertions(+), 10 deletions(-)

diff --git a/wpimath/src/main/native/cpp/geometry/Translation2d.cpp b/wpimath/src/main/native/cpp/geometry/Translation2d.cpp
index 268c139a313..88d265d717c 100644
--- a/wpimath/src/main/native/cpp/geometry/Translation2d.cpp
+++ b/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);
 }
diff --git a/wpimath/src/main/native/cpp/geometry/Translation3d.cpp b/wpimath/src/main/native/cpp/geometry/Translation3d.cpp
index b422c37d0f4..c8c31d0c616 100644
--- a/wpimath/src/main/native/cpp/geometry/Translation3d.cpp
+++ b/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) +
diff --git a/wpimath/src/main/native/include/frc/geometry/Translation2d.h b/wpimath/src/main/native/include/frc/geometry/Translation2d.h
index 0e389c7effe..fee42f13de1 100644
--- a/wpimath/src/main/native/include/frc/geometry/Translation2d.h
+++ b/wpimath/src/main/native/include/frc/geometry/Translation2d.h
@@ -55,7 +55,7 @@ class WPILIB_DLLEXPORT Translation2d {
    *
    * @param vector The translation vector to represent.
    */
-  constexpr Translation2d(const Eigen::Vector2d& vector);
+  Translation2d(const Eigen::Vector2d& vector);
 
   /**
    * Calculates the distance between two translations in 2D space.
diff --git a/wpimath/src/main/native/include/frc/geometry/Translation2d.inc b/wpimath/src/main/native/include/frc/geometry/Translation2d.inc
index 297f96558cb..3be897fad6d 100644
--- a/wpimath/src/main/native/include/frc/geometry/Translation2d.inc
+++ b/wpimath/src/main/native/include/frc/geometry/Translation2d.inc
@@ -16,9 +16,6 @@ constexpr Translation2d::Translation2d(units::meter_t distance,
                                        const Rotation2d& angle)
     : m_x(distance * angle.Cos()), m_y(distance * angle.Sin()) {}
 
-constexpr Translation2d::Translation2d(const Eigen::Vector2d& vector)
-    : m_x(units::meter_t(vector.x())), m_y(units::meter_t(vector.y())) {}
-
 constexpr Rotation2d Translation2d::Angle() const {
   return Rotation2d{m_x.value(), m_y.value()};
 }
diff --git a/wpimath/src/main/native/include/frc/geometry/Translation3d.h b/wpimath/src/main/native/include/frc/geometry/Translation3d.h
index 782d0a80778..8d341d63a2a 100644
--- a/wpimath/src/main/native/include/frc/geometry/Translation3d.h
+++ b/wpimath/src/main/native/include/frc/geometry/Translation3d.h
@@ -54,7 +54,7 @@ class WPILIB_DLLEXPORT Translation3d {
    *
    * @param vector The translation vector to represent.
    */
-  constexpr Translation3d(const Eigen::Vector3d& vector);
+  Translation3d(const Eigen::Vector3d& vector);
 
   /**
    * Calculates the distance between two translations in 3D space.
diff --git a/wpimath/src/main/native/include/frc/geometry/Translation3d.inc b/wpimath/src/main/native/include/frc/geometry/Translation3d.inc
index 5b17af597e7..8ab6e94c609 100644
--- a/wpimath/src/main/native/include/frc/geometry/Translation3d.inc
+++ b/wpimath/src/main/native/include/frc/geometry/Translation3d.inc
@@ -15,11 +15,6 @@ constexpr Translation3d::Translation3d(units::meter_t x, units::meter_t y,
                                        units::meter_t z)
     : m_x(x), m_y(y), m_z(z) {}
 
-constexpr 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())) {}
-
 constexpr Translation2d Translation3d::ToTranslation2d() const {
   return Translation2d{m_x, m_y};
 }

From 14f040297f5c6d0febba678fb4f7dac1cba5af0c Mon Sep 17 00:00:00 2001
From: Asa Paparo <asapaparo@gmail.com>
Date: Sun, 4 Feb 2024 17:46:46 -0500
Subject: [PATCH 08/19] make translation vector ctors explicit

---
 wpimath/src/main/native/include/frc/geometry/Translation2d.h | 2 +-
 wpimath/src/main/native/include/frc/geometry/Translation3d.h | 2 +-
 2 files changed, 2 insertions(+), 2 deletions(-)

diff --git a/wpimath/src/main/native/include/frc/geometry/Translation2d.h b/wpimath/src/main/native/include/frc/geometry/Translation2d.h
index fee42f13de1..51132de626e 100644
--- a/wpimath/src/main/native/include/frc/geometry/Translation2d.h
+++ b/wpimath/src/main/native/include/frc/geometry/Translation2d.h
@@ -55,7 +55,7 @@ class WPILIB_DLLEXPORT Translation2d {
    *
    * @param vector The translation vector to represent.
    */
-  Translation2d(const Eigen::Vector2d& vector);
+  explicit Translation2d(const Eigen::Vector2d& vector);
 
   /**
    * Calculates the distance between two translations in 2D space.
diff --git a/wpimath/src/main/native/include/frc/geometry/Translation3d.h b/wpimath/src/main/native/include/frc/geometry/Translation3d.h
index 8d341d63a2a..23ddd7cd376 100644
--- a/wpimath/src/main/native/include/frc/geometry/Translation3d.h
+++ b/wpimath/src/main/native/include/frc/geometry/Translation3d.h
@@ -54,7 +54,7 @@ class WPILIB_DLLEXPORT Translation3d {
    *
    * @param vector The translation vector to represent.
    */
-  Translation3d(const Eigen::Vector3d& vector);
+  explicit Translation3d(const Eigen::Vector3d& vector);
 
   /**
    * Calculates the distance between two translations in 3D space.

From 92dbf55a77d15a4dc473197caa734263a92f88ad Mon Sep 17 00:00:00 2001
From: Asa Paparo <asapaparo@gmail.com>
Date: Sun, 4 Feb 2024 18:02:38 -0500
Subject: [PATCH 09/19] remove Vector.comp, add testVectorUnit

---
 .../src/main/java/edu/wpi/first/math/Vector.java   | 14 ++------------
 .../test/java/edu/wpi/first/math/VectorTest.java   | 10 ++++++++--
 2 files changed, 10 insertions(+), 14 deletions(-)

diff --git a/wpimath/src/main/java/edu/wpi/first/math/Vector.java b/wpimath/src/main/java/edu/wpi/first/math/Vector.java
index f91158c7e10..895fa14df80 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/Vector.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/Vector.java
@@ -127,23 +127,13 @@ public Vector<R> unit() {
     return div(norm());
   }
 
-  /**
-   * Returns the scalar component of this vector along another.
-   *
-   * @param other The vector to find the component of this along.
-   * @return The component.
-   */
-  public double component(Vector<R> other) {
-    return dot(other) / other.norm();
-  }
-
   /**
    * 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.unit().times(component(other));
+  public Vector<R> proj(Vector<R> other) {
+    return other.times(dot(other)).div(other.dot(other));
   }
 }
diff --git a/wpimath/src/test/java/edu/wpi/first/math/VectorTest.java b/wpimath/src/test/java/edu/wpi/first/math/VectorTest.java
index 2a51b8e07c7..1191c2a356a 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/VectorTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/VectorTest.java
@@ -66,11 +66,17 @@ void testVectorNorm() {
     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.projection(vec2);
+    var res1 = vec1.proj(vec2);
 
     assertEquals(res1.get(0), 1.662, 0.01);
     assertEquals(res1.get(1), 2.077, 0.01);
@@ -78,7 +84,7 @@ void testVectorProj() {
 
     var vec3 = VecBuilder.fill(-1.0, 2.0, -3.0);
     var vec4 = VecBuilder.fill(4.0, -5.0, 6.0);
-    var res2 = vec4.projection(vec3);
+    var res2 = vec4.proj(vec3);
 
     assertEquals(res2.get(0), 2.29, 0.01);
     assertEquals(res2.get(1), -4.57, 0.01);

From a7f80b0309341540088f2aedaa9e32eed609553d Mon Sep 17 00:00:00 2001
From: Asa Paparo <asapaparo@gmail.com>
Date: Sun, 4 Feb 2024 20:13:57 -0500
Subject: [PATCH 10/19] Update
 wpimath/src/test/java/edu/wpi/first/math/geometry/Translation3dTest.java

Co-authored-by: Tyler Veness <calcmogul@gmail.com>
---
 .../java/edu/wpi/first/math/geometry/Translation3dTest.java     | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/Translation3dTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/Translation3dTest.java
index 9d6137761d4..3dd54227ea8 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/geometry/Translation3dTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/Translation3dTest.java
@@ -162,7 +162,7 @@ void testPolarConstructor() {
   }
 
   @Test
-  void testVector() {
+  void testToVector() {
     var vec = VecBuilder.fill(1.0, 2.0, 3.0);
     var translation = new Translation3d(vec);
 

From 53a095f3ca4d070c9c9d057b8ed2c8b51065ef79 Mon Sep 17 00:00:00 2001
From: Asa Paparo <asapaparo@gmail.com>
Date: Sun, 4 Feb 2024 20:14:06 -0500
Subject: [PATCH 11/19] Update
 wpimath/src/test/java/edu/wpi/first/math/geometry/Translation2dTest.java

Co-authored-by: Tyler Veness <calcmogul@gmail.com>
---
 .../java/edu/wpi/first/math/geometry/Translation2dTest.java     | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/Translation2dTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/Translation2dTest.java
index df888ff6d5a..39e2073b638 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/geometry/Translation2dTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/Translation2dTest.java
@@ -134,7 +134,7 @@ void testNearest() {
   }
 
   @Test
-  void testVector() {
+  void testToVector() {
     var vec = VecBuilder.fill(1.0, 2.0);
     var translation = new Translation2d(vec);
 

From 44783f40241cceec3e957b9a71ef754ee382347b Mon Sep 17 00:00:00 2001
From: Asa Paparo <asapaparo@gmail.com>
Date: Sun, 4 Feb 2024 20:15:01 -0500
Subject: [PATCH 12/19] Apply suggestions from code review

Co-authored-by: Tyler Veness <calcmogul@gmail.com>
---
 wpimath/src/main/java/edu/wpi/first/math/Vector.java     | 2 +-
 wpimath/src/test/java/edu/wpi/first/math/VectorTest.java | 6 +++---
 2 files changed, 4 insertions(+), 4 deletions(-)

diff --git a/wpimath/src/main/java/edu/wpi/first/math/Vector.java b/wpimath/src/main/java/edu/wpi/first/math/Vector.java
index 895fa14df80..3a10d123ee6 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/Vector.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/Vector.java
@@ -133,7 +133,7 @@ public Vector<R> unit() {
    * @param other The vector to project along.
    * @return The projection.
    */
-  public Vector<R> proj(Vector<R> other) {
+  public Vector<R> projection(Vector<R> other) {
     return other.times(dot(other)).div(other.dot(other));
   }
 }
diff --git a/wpimath/src/test/java/edu/wpi/first/math/VectorTest.java b/wpimath/src/test/java/edu/wpi/first/math/VectorTest.java
index 1191c2a356a..07233cfaa70 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/VectorTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/VectorTest.java
@@ -73,10 +73,10 @@ void testVectorUnit() {
   }
 
   @Test
-  void testVectorProj() {
+  void testVectorProjection() {
     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);
+    var res1 = vec1.projection(vec2);
 
     assertEquals(res1.get(0), 1.662, 0.01);
     assertEquals(res1.get(1), 2.077, 0.01);
@@ -84,7 +84,7 @@ void testVectorProj() {
 
     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);
+    var res2 = vec4.projection(vec3);
 
     assertEquals(res2.get(0), 2.29, 0.01);
     assertEquals(res2.get(1), -4.57, 0.01);

From ed750d92432310968bee6f98cd42b8d79db4eaea Mon Sep 17 00:00:00 2001
From: Asa Paparo <asapaparo@gmail.com>
Date: Mon, 5 Feb 2024 11:42:37 -0500
Subject: [PATCH 13/19] add c++ rotation3d test

---
 .../test/native/cpp/geometry/Translation3dTest.cpp    | 11 +++++++++++
 1 file changed, 11 insertions(+)

diff --git a/wpimath/src/test/native/cpp/geometry/Translation3dTest.cpp b/wpimath/src/test/native/cpp/geometry/Translation3dTest.cpp
index b7aa8ceeff6..56eaf6da753 100644
--- a/wpimath/src/test/native/cpp/geometry/Translation3dTest.cpp
+++ b/wpimath/src/test/native/cpp/geometry/Translation3dTest.cpp
@@ -128,6 +128,17 @@ TEST(Translation3dTest, PolarConstructor) {
   EXPECT_NEAR(two.Z().value(), 0.0, kEpsilon);
 }
 
+TEST(Translation3dTest, ToVector) {
+  const Eigen::Vector3d vec(1.0, 2.0, 3.0);
+  const Translation3d translation{vec};
+
+  EXPECT_DOUBLE_EQ(vec[0], translation.X().value());
+  EXPECT_DOUBLE_EQ(vec[1], translation.Y().value());
+  EXPECT_DOUBLE_EQ(vec[2], translation.Z().value());
+
+  EXPECT_TRUE(vec == translation.ToVector());
+}
+
 TEST(Translation3dTest, Constexpr) {
   constexpr Translation3d defaultCtor;
   constexpr Translation3d componentCtor{1_m, 2_m, 3_m};

From 4e201cb285ab9b7388334eaa1e3bf06b61971d79 Mon Sep 17 00:00:00 2001
From: Asa Paparo <asapaparo@gmail.com>
Date: Mon, 5 Feb 2024 14:09:38 -0500
Subject: [PATCH 14/19] addressed reviews

---
 wpimath/src/main/native/cpp/geometry/Translation2d.cpp |  6 +-----
 wpimath/src/main/native/cpp/geometry/Translation3d.cpp | 10 +++-------
 .../main/native/include/frc/geometry/Translation2d.h   |  2 +-
 .../main/native/include/frc/geometry/Translation2d.inc |  4 ++++
 .../main/native/include/frc/geometry/Translation3d.h   |  2 +-
 .../main/native/include/frc/geometry/Translation3d.inc |  4 ++++
 6 files changed, 14 insertions(+), 14 deletions(-)

diff --git a/wpimath/src/main/native/cpp/geometry/Translation2d.cpp b/wpimath/src/main/native/cpp/geometry/Translation2d.cpp
index 88d265d717c..90162018ed7 100644
--- a/wpimath/src/main/native/cpp/geometry/Translation2d.cpp
+++ b/wpimath/src/main/native/cpp/geometry/Translation2d.cpp
@@ -12,16 +12,12 @@
 using namespace frc;
 
 Translation2d::Translation2d(const Eigen::Vector2d& vector)
-    : m_x(units::meter_t(vector.x())), m_y(units::meter_t(vector.y())) {}
+    : 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);
 }
diff --git a/wpimath/src/main/native/cpp/geometry/Translation3d.cpp b/wpimath/src/main/native/cpp/geometry/Translation3d.cpp
index c8c31d0c616..2da61379f2b 100644
--- a/wpimath/src/main/native/cpp/geometry/Translation3d.cpp
+++ b/wpimath/src/main/native/cpp/geometry/Translation3d.cpp
@@ -20,9 +20,9 @@ Translation3d::Translation3d(units::meter_t distance, const Rotation3d& angle) {
 }
 
 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())) {}
+    : 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) +
@@ -30,10 +30,6 @@ units::meter_t Translation3d::Distance(const Translation3d& other) const {
                            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);
 }
diff --git a/wpimath/src/main/native/include/frc/geometry/Translation2d.h b/wpimath/src/main/native/include/frc/geometry/Translation2d.h
index 51132de626e..9d9a77c3bf2 100644
--- a/wpimath/src/main/native/include/frc/geometry/Translation2d.h
+++ b/wpimath/src/main/native/include/frc/geometry/Translation2d.h
@@ -87,7 +87,7 @@ class WPILIB_DLLEXPORT Translation2d {
    *
    * @return A Vector representation of this translation.
    */
-  Eigen::Vector2d ToVector() const;
+  constexpr Eigen::Vector2d ToVector() const;
 
   /**
    * Returns the norm, or distance from the origin to the translation.
diff --git a/wpimath/src/main/native/include/frc/geometry/Translation2d.inc b/wpimath/src/main/native/include/frc/geometry/Translation2d.inc
index 3be897fad6d..6e1ddf0ac1b 100644
--- a/wpimath/src/main/native/include/frc/geometry/Translation2d.inc
+++ b/wpimath/src/main/native/include/frc/geometry/Translation2d.inc
@@ -16,6 +16,10 @@ constexpr Translation2d::Translation2d(units::meter_t distance,
                                        const Rotation2d& angle)
     : 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()};
 }
diff --git a/wpimath/src/main/native/include/frc/geometry/Translation3d.h b/wpimath/src/main/native/include/frc/geometry/Translation3d.h
index 23ddd7cd376..b83b661fd98 100644
--- a/wpimath/src/main/native/include/frc/geometry/Translation3d.h
+++ b/wpimath/src/main/native/include/frc/geometry/Translation3d.h
@@ -94,7 +94,7 @@ class WPILIB_DLLEXPORT Translation3d {
    *
    * @return A Vector representation of this translation.
    */
-  Eigen::Vector3d ToVector() const;
+  constexpr Eigen::Vector3d ToVector() const;
 
   /**
    * Returns the norm, or distance from the origin to the translation.
diff --git a/wpimath/src/main/native/include/frc/geometry/Translation3d.inc b/wpimath/src/main/native/include/frc/geometry/Translation3d.inc
index 8ab6e94c609..b2d188d284f 100644
--- a/wpimath/src/main/native/include/frc/geometry/Translation3d.inc
+++ b/wpimath/src/main/native/include/frc/geometry/Translation3d.inc
@@ -19,6 +19,10 @@ 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()};

From c0f54d0dc133d88ea533926ea3f64ae6452fca3e Mon Sep 17 00:00:00 2001
From: Asa Paparo <asapaparo@gmail.com>
Date: Mon, 5 Feb 2024 14:29:45 -0500
Subject: [PATCH 15/19] consistently used list initialization in c++ geo

---
 wpimath/src/main/native/cpp/geometry/Pose3d.cpp           | 8 ++++----
 wpimath/src/main/native/cpp/geometry/Transform3d.cpp      | 4 ++--
 wpimath/src/main/native/include/frc/geometry/Pose2d.inc   | 4 ++--
 .../src/main/native/include/frc/geometry/Rotation2d.inc   | 8 ++++----
 .../src/main/native/include/frc/geometry/Transform2d.inc  | 4 ++--
 .../main/native/include/frc/geometry/Translation2d.inc    | 4 ++--
 .../main/native/include/frc/geometry/Translation3d.inc    | 2 +-
 7 files changed, 17 insertions(+), 17 deletions(-)

diff --git a/wpimath/src/main/native/cpp/geometry/Pose3d.cpp b/wpimath/src/main/native/cpp/geometry/Pose3d.cpp
index 90f7b491f5a..bd7ab8681e8 100644
--- a/wpimath/src/main/native/cpp/geometry/Pose3d.cpp
+++ b/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);
diff --git a/wpimath/src/main/native/cpp/geometry/Transform3d.cpp b/wpimath/src/main/native/cpp/geometry/Transform3d.cpp
index 556a3027d0b..4879bb12ad5 100644
--- a/wpimath/src/main/native/cpp/geometry/Transform3d.cpp
+++ b/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
diff --git a/wpimath/src/main/native/include/frc/geometry/Pose2d.inc b/wpimath/src/main/native/include/frc/geometry/Pose2d.inc
index 2764d16cbf8..559a0034e80 100644
--- a/wpimath/src/main/native/include/frc/geometry/Pose2d.inc
+++ b/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);
diff --git a/wpimath/src/main/native/include/frc/geometry/Rotation2d.inc b/wpimath/src/main/native/include/frc/geometry/Rotation2d.inc
index a222b362317..104b66b1605 100644
--- a/wpimath/src/main/native/include/frc/geometry/Rotation2d.inc
+++ b/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 {
diff --git a/wpimath/src/main/native/include/frc/geometry/Transform2d.inc b/wpimath/src/main/native/include/frc/geometry/Transform2d.inc
index 862b8d5c600..cc925148d45 100644
--- a/wpimath/src/main/native/include/frc/geometry/Transform2d.inc
+++ b/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
diff --git a/wpimath/src/main/native/include/frc/geometry/Translation2d.inc b/wpimath/src/main/native/include/frc/geometry/Translation2d.inc
index 6e1ddf0ac1b..6b291e1f74c 100644
--- a/wpimath/src/main/native/include/frc/geometry/Translation2d.inc
+++ b/wpimath/src/main/native/include/frc/geometry/Translation2d.inc
@@ -10,11 +10,11 @@
 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()}};
diff --git a/wpimath/src/main/native/include/frc/geometry/Translation3d.inc b/wpimath/src/main/native/include/frc/geometry/Translation3d.inc
index b2d188d284f..19268e6cee2 100644
--- a/wpimath/src/main/native/include/frc/geometry/Translation3d.inc
+++ b/wpimath/src/main/native/include/frc/geometry/Translation3d.inc
@@ -13,7 +13,7 @@ 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};

From 4455042832f1c5f005deddef7f943ee03fac0d67 Mon Sep 17 00:00:00 2001
From: Asa Paparo <asapaparo@gmail.com>
Date: Mon, 5 Feb 2024 15:23:46 -0500
Subject: [PATCH 16/19] add java Vector<N2> cross and tests

---
 .../src/main/java/edu/wpi/first/math/Vector.java | 16 ++++++++++++++++
 .../test/java/edu/wpi/first/math/VectorTest.java | 11 +++++++++++
 2 files changed, 27 insertions(+)

diff --git a/wpimath/src/main/java/edu/wpi/first/math/Vector.java b/wpimath/src/main/java/edu/wpi/first/math/Vector.java
index 3a10d123ee6..0fc76cdb18d 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/Vector.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/Vector.java
@@ -5,7 +5,9 @@
 package edu.wpi.first.math;
 
 import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.numbers.N3;
 import java.util.Objects;
+import java.util.Vector;
 import org.ejml.simple.SimpleMatrix;
 
 /**
@@ -136,4 +138,18 @@ public Vector<R> unit() {
   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));
+  }
 }
diff --git a/wpimath/src/test/java/edu/wpi/first/math/VectorTest.java b/wpimath/src/test/java/edu/wpi/first/math/VectorTest.java
index 07233cfaa70..7b6edefe662 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/VectorTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/VectorTest.java
@@ -90,4 +90,15 @@ void testVectorProjection() {
     assertEquals(res2.get(1), -4.57, 0.01);
     assertEquals(res2.get(2), 6.86, 0.01);
   }
+
+  @Test
+  void testVectorCross() {
+    var e1 = VecBuilder.fill(1.0, 0.0, 0.0);
+    var e2 = VecBuilder.fill(0.0, 1.0, 0.0);
+    assertEquals(Vector.cross(e1, e2), VecBuilder.fill(0.0, 0.0, 1.0));
+
+    var vec1 = VecBuilder.fill(1.0, 2.0, 3.0);
+    var vec2 = VecBuilder.fill(3.0, 4.0, 5.0);
+    assertEquals(Vector.cross(vec1, vec2), VecBuilder.fill(-2.0, 4.0, -2.0));
+  }
 }

From c6c7c4fbc25454b6a544ad32600b7ea4f4fcae18 Mon Sep 17 00:00:00 2001
From: "github-actions[bot]"
 <41898282+github-actions[bot]@users.noreply.github.com>
Date: Mon, 5 Feb 2024 20:28:47 +0000
Subject: [PATCH 17/19] Formatting fixes

---
 wpimath/src/main/native/cpp/geometry/Translation3d.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/wpimath/src/main/native/cpp/geometry/Translation3d.cpp b/wpimath/src/main/native/cpp/geometry/Translation3d.cpp
index 2da61379f2b..c3b8cb4b775 100644
--- a/wpimath/src/main/native/cpp/geometry/Translation3d.cpp
+++ b/wpimath/src/main/native/cpp/geometry/Translation3d.cpp
@@ -22,7 +22,7 @@ Translation3d::Translation3d(units::meter_t distance, const Rotation3d& angle) {
 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()}} {}  
+      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) +

From 0d4158380d6cac57ca2b72f98418373a1f5499c7 Mon Sep 17 00:00:00 2001
From: Asa Paparo <asapaparo@gmail.com>
Date: Mon, 5 Feb 2024 20:24:19 -0500
Subject: [PATCH 18/19] removed stray import

---
 wpimath/src/main/java/edu/wpi/first/math/Vector.java | 1 -
 1 file changed, 1 deletion(-)

diff --git a/wpimath/src/main/java/edu/wpi/first/math/Vector.java b/wpimath/src/main/java/edu/wpi/first/math/Vector.java
index 0fc76cdb18d..8784a6c027b 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/Vector.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/Vector.java
@@ -7,7 +7,6 @@
 import edu.wpi.first.math.numbers.N1;
 import edu.wpi.first.math.numbers.N3;
 import java.util.Objects;
-import java.util.Vector;
 import org.ejml.simple.SimpleMatrix;
 
 /**

From 99d5033d766025cc5196f2f125fb50c89345fddf Mon Sep 17 00:00:00 2001
From: Asa Paparo <asapaparo@gmail.com>
Date: Mon, 5 Feb 2024 22:45:58 -0500
Subject: [PATCH 19/19] tyler's CI cmake changes

---
 .github/workflows/cmake.yml | 2 +-
 vcpkg.json                  | 1 -
 2 files changed, 1 insertion(+), 2 deletions(-)

diff --git a/.github/workflows/cmake.yml b/.github/workflows/cmake.yml
index 6db3659e1ad..f576a13c4bb 100644
--- a/.github/workflows/cmake.yml
+++ b/.github/workflows/cmake.yml
@@ -91,7 +91,7 @@ jobs:
           vcpkgGitCommitId: 78b61582c9e093fda56a01ebb654be15a0033897 # HEAD on 2023-08-6
 
       - name: configure
-        run: cmake -S . -B build -G "Ninja" -DCMAKE_C_COMPILER_LAUNCHER=sccache -DCMAKE_CXX_COMPILER_LAUNCHER=sccache -DCMAKE_BUILD_TYPE=Release -DWITH_JAVA=OFF -DWITH_EXAMPLES=ON -DUSE_SYSTEM_FMTLIB=ON -DUSE_SYSTEM_LIBUV=ON -DUSE_SYSTEM_EIGEN=ON -DCMAKE_TOOLCHAIN_FILE=${{ runner.workspace }}/vcpkg/scripts/buildsystems/vcpkg.cmake -DVCPKG_INSTALL_OPTIONS=--clean-after-build -DVCPKG_TARGET_TRIPLET=x64-windows-release -DVCPKG_HOST_TRIPLET=x64-windows-release
+        run: cmake -S . -B build -G "Ninja" -DCMAKE_C_COMPILER_LAUNCHER=sccache -DCMAKE_CXX_COMPILER_LAUNCHER=sccache -DCMAKE_BUILD_TYPE=Release -DWITH_JAVA=OFF -DWITH_EXAMPLES=ON -DUSE_SYSTEM_FMTLIB=ON -DUSE_SYSTEM_LIBUV=ON -DUSE_SYSTEM_EIGEN=OFF -DCMAKE_TOOLCHAIN_FILE=${{ runner.workspace }}/vcpkg/scripts/buildsystems/vcpkg.cmake -DVCPKG_INSTALL_OPTIONS=--clean-after-build -DVCPKG_TARGET_TRIPLET=x64-windows-release -DVCPKG_HOST_TRIPLET=x64-windows-release
         env:
           SCCACHE_GHA_ENABLED: "true"
 
diff --git a/vcpkg.json b/vcpkg.json
index ea60ac26059..29af026936a 100644
--- a/vcpkg.json
+++ b/vcpkg.json
@@ -3,7 +3,6 @@
   "version-string": "latest",
   "dependencies": [
     "opencv",
-    "eigen3",
     "fmt",
     "libuv",
     "protobuf"