From 8aedbd33dcbf26fbc6b0f7f51db3a5a928bb5d17 Mon Sep 17 00:00:00 2001
From: aarav18 <aaravshah18@gmail.com>
Date: Wed, 16 Mar 2022 14:33:40 -0600
Subject: [PATCH] Create Vector2D.java

---
 src/main/java/frc4388/utility/Vector2D.java | 101 ++++++++++++++++++++
 1 file changed, 101 insertions(+)
 create mode 100644 src/main/java/frc4388/utility/Vector2D.java

diff --git a/src/main/java/frc4388/utility/Vector2D.java b/src/main/java/frc4388/utility/Vector2D.java
new file mode 100644
index 0000000..1b76bb2
--- /dev/null
+++ b/src/main/java/frc4388/utility/Vector2D.java
@@ -0,0 +1,101 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc4388.utility;
+
+import edu.wpi.first.wpilibj.drive.Vector2d;
+
+/** Aarav's good vector class (better than WPILib) */
+public class Vector2D extends Vector2d {
+
+  public double x;
+  public double y;
+  public double angle;
+
+  public Vector2D() {
+    super();
+    this.angle = Math.atan2(this.y, this.x);
+  }
+
+  public Vector2D(double x, double y) {
+    super(x, y);
+
+    this.x = x;
+    this.y = y;
+    this.angle = Math.atan2(this.y, this.x);
+  }
+
+  /**
+   * Add two vectors, component-wise.
+   * @param v1 First vector in the addition.
+   * @param v2 Second vector in the addition.
+   * @return New vector which is the sum.
+   */
+  public static Vector2D add(Vector2D v1, Vector2D v2) {
+    return new Vector2D(v1.x + v2.x, v1.y + v2.y);
+  }
+
+  /**
+   * Subtract two vectors, component-wise.
+   * @param v1 First vector in the subtraction.
+   * @param v2 Second vector in the subtraction.
+   * @return New vector which is the difference.
+   */
+  public static Vector2D subtract(Vector2D v1, Vector2D v2) {
+    return new Vector2D(v1.x - v2.x, v1.y - v2.y);
+  }
+
+  /**
+   * Multiply a vector with a scalar, component-wise.
+   * @param v1 Vector to multiply.
+   * @param v2 Scalar to multiply.
+   * @return New vector which is the product.
+   */
+  public static Vector2D multiply(Vector2D v1, double scalar) {
+    return new Vector2D(scalar * v1.x, scalar * v1.y);
+  }
+
+    /**
+   * Divide a vector with a scalar, component-wise.
+   * @param v1 Vector to divide.
+   * @param v2 Scalar to divide.
+   * @return New vector which is the division.
+   */
+  public static Vector2D divide(Vector2D v1, double scalar) {
+    return new Vector2D(v1.x / scalar, v1.y / scalar);
+  }
+
+  /**
+   * Find unit vector.
+   * @return The unit vector.
+   */
+  public Vector2D unit() {
+    return new Vector2D(this.x / this.magnitude(), this.y / this.magnitude());
+  }
+
+  /**
+   * Round a vector to a certain number of places, component-wise.
+   * @param v Vector to round.
+   * @param places Number of places to round to.
+   * @return New rounded vector.
+   */
+  public static Vector2D round(Vector2D v, int places) {
+    int scale = (int) Math.pow(10, places);
+
+    v = Vector2D.multiply(v, scale);
+
+    v.x = Math.round(v.x);
+    v.y = Math.round(v.y);
+    v.x = v.x / scale;
+    v.y = v.y / scale;
+
+    return v;
+  }
+
+  @Override
+  public String toString() {
+    return ("(" + this.x + ", " + this.y + ")");
+  }
+
+}