[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,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));
   }
 }

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));

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

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