From a13b183107c06eda67c5a27562374d56867da1ec Mon Sep 17 00:00:00 2001
From: Tyler Veness <calcmogul@gmail.com>
Date: Tue, 17 Oct 2023 11:11:01 -0700
Subject: [PATCH] [wpimath] Make Vector-Vector binary operators return Vector

Fixes #5741.
---
 .../main/java/edu/wpi/first/math/Vector.java  | 21 ++++++++++
 .../java/edu/wpi/first/math/VectorTest.java   | 38 +++++++++++++++++++
 2 files changed, 59 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 1aebdde4a1d..bc467411259 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/Vector.java
+++ b/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 java.util.Objects;
 import org.ejml.simple.SimpleMatrix;
 
 /**
@@ -62,6 +63,26 @@ public Vector<R> div(double value) {
     return new Vector<>(this.m_storage.divide(value));
   }
 
+  /**
+   * Adds the given vector to this vector.
+   *
+   * @param value The vector to add.
+   * @return The resultant vector.
+   */
+  public final Vector<R> plus(Vector<R> value) {
+    return new Vector<>(this.m_storage.plus(Objects.requireNonNull(value).m_storage));
+  }
+
+  /**
+   * Subtracts the given vector to this vector.
+   *
+   * @param value The vector to add.
+   * @return The resultant vector.
+   */
+  public final Vector<R> minus(Vector<R> value) {
+    return new Vector<>(this.m_storage.minus(Objects.requireNonNull(value).m_storage));
+  }
+
   /**
    * Returns the dot product of this vector with another.
    *
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 5488b68449c..008953f9170 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/VectorTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/VectorTest.java
@@ -9,6 +9,44 @@
 import org.junit.jupiter.api.Test;
 
 class VectorTest {
+  @Test
+  void testVectorPlus() {
+    var vec1 = VecBuilder.fill(1.0, 2.0, 3.0);
+    var vec2 = VecBuilder.fill(4.0, 5.0, 6.0);
+    var result1 = vec1.plus(vec2);
+
+    assertEquals(5.0, result1.get(0, 0));
+    assertEquals(7.0, result1.get(1, 0));
+    assertEquals(9.0, result1.get(2, 0));
+
+    var vec3 = VecBuilder.fill(-1.0, 2.0, -3.0);
+    var vec4 = VecBuilder.fill(4.0, -5.0, 6.0);
+    var result2 = vec3.plus(vec4);
+
+    assertEquals(3.0, result2.get(0, 0));
+    assertEquals(-3.0, result2.get(1, 0));
+    assertEquals(3.0, result2.get(2, 0));
+  }
+
+  @Test
+  void testVectorMinus() {
+    var vec1 = VecBuilder.fill(1.0, 2.0, 3.0);
+    var vec2 = VecBuilder.fill(4.0, 5.0, 6.0);
+    var result1 = vec1.minus(vec2);
+
+    assertEquals(-3.0, result1.get(0, 0));
+    assertEquals(-3.0, result1.get(1, 0));
+    assertEquals(-3.0, result1.get(2, 0));
+
+    var vec3 = VecBuilder.fill(-1.0, 2.0, -3.0);
+    var vec4 = VecBuilder.fill(4.0, -5.0, 6.0);
+    var result2 = vec3.minus(vec4);
+
+    assertEquals(-5.0, result2.get(0, 0));
+    assertEquals(7.0, result2.get(1, 0));
+    assertEquals(-9.0, result2.get(2, 0));
+  }
+
   @Test
   void testVectorDot() {
     var vec1 = VecBuilder.fill(1.0, 2.0, 3.0);