[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.
    *