From 13e3b1e0568f636f3f2ea6c995eeb26cb5075669 Mon Sep 17 00:00:00 2001
From: Ryan Manley <aaravshah18@gmail.com>
Date: Mon, 31 Jan 2022 19:39:33 -0700
Subject: [PATCH] Added matrix determinant

---
 .../commands/drive/VisionUpdateOdometry.java  | 34 +++++++++++++++++++
 vendordeps/{REVRobotics.json => REVLib.json}  | 30 ++++++++--------
 2 files changed, 49 insertions(+), 15 deletions(-)
 rename vendordeps/{REVRobotics.json => REVLib.json} (72%)

diff --git a/src/main/java/frc4388/robot/commands/drive/VisionUpdateOdometry.java b/src/main/java/frc4388/robot/commands/drive/VisionUpdateOdometry.java
index 06877c2..b97b83d 100644
--- a/src/main/java/frc4388/robot/commands/drive/VisionUpdateOdometry.java
+++ b/src/main/java/frc4388/robot/commands/drive/VisionUpdateOdometry.java
@@ -3,11 +3,20 @@ package frc4388.robot.commands.drive;
 import com.ctre.phoenix.sensors.PigeonIMU;
 import com.revrobotics.CANDigitalInput.LimitSwitchPolarity;
 
+import org.ejml.simple.SimpleMatrix;
+import org.opencv.core.Mat;
+
 import edu.wpi.first.networktables.NetworkTableInstance;
 import edu.wpi.first.wpilibj.geometry.Pose2d;
 import edu.wpi.first.wpilibj.geometry.Rotation2d;
 import edu.wpi.first.wpilibj.geometry.Translation2d;
 import edu.wpi.first.wpilibj2.command.CommandBase;
+import edu.wpi.first.wpiutil.math.Matrix;
+import edu.wpi.first.wpiutil.math.Nat;
+import edu.wpi.first.wpiutil.math.Num;
+import edu.wpi.first.wpiutil.math.VecBuilder;
+import edu.wpi.first.wpiutil.math.numbers.N5;
+import edu.wpi.first.wpiutil.math.numbers.N6;
 import frc4388.robot.Constants.VOPConstants;
 import frc4388.robot.Constants.VisionConstants;
 import frc4388.robot.subsystems.Drive;
@@ -74,7 +83,32 @@ public class VisionUpdateOdometry extends CommandBase {
   // http://www.lee-mac.com/5pointellipse.html
   // https://math.stackexchange.com/questions/163920/how-to-find-an-ellipse-given-five-points
   // https://towardsdatascience.com/understanding-singular-value-decomposition-and-its-application-in-data-science-388a54be95d
+  // https://www.desmos.com/calculator/ounqguxjac
+  // https://en.wikipedia.org/wiki/Five_points_determine_a_conic
+
+  /* solves the following matrix
+   *    | x0^2 x0y0 y0^2 x0 y0 1 |
+   *    | x1^2 x1y1 y1^2 x1 y1 1 |
+   * det| x2^2 x2y2 y2^2 x2 y2 1 | = 0
+   *    | x3^2 x3y3 y3^2 x3 y3 1 |
+   *    | x4^2 x4y4 y4^2 x4 y4 1 |
+   *    | x5^2 x5y5 y5^2 x5 y5 1 |
+   * for conic equation
+   * ax^2 - bxy + cy^2 - dx + fy - g = 0
+   */
   public static double[] getEllipseRadii(double[] xPoints, double[] yPoints) {
+    double[][] matrix = new double[5][6];
+
+    // Generate matrix
+    for(int i = 0; i < 5; i++) {
+      matrix[i][0] = xPoints[i] * xPoints[i];
+      matrix[i][1] = xPoints[i] * yPoints[i];
+      matrix[i][2] = yPoints[i] * yPoints[i];
+      matrix[i][3] = xPoints[i] * 1.d;
+      matrix[i][4] = 1.d * yPoints[i];
+      matrix[i][5] = 1.d;
+    }
+
     return null;
   }
 
diff --git a/vendordeps/REVRobotics.json b/vendordeps/REVLib.json
similarity index 72%
rename from vendordeps/REVRobotics.json
rename to vendordeps/REVLib.json
index ca33a31..997e2a4 100644
--- a/vendordeps/REVRobotics.json
+++ b/vendordeps/REVLib.json
@@ -1,24 +1,24 @@
 {
-    "fileName": "REVRobotics.json",
-    "name": "REVRobotics",
-    "version": "1.5.4",
+    "fileName": "REVLib.json",
+    "name": "REVLib",
+    "version": "2022.1.1",
     "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb",
     "mavenUrls": [
-        "http://www.revrobotics.com/content/sw/max/sdk/maven/"
+        "https://maven.revrobotics.com/"
     ],
-    "jsonUrl": "http://www.revrobotics.com/content/sw/max/sdk/REVRobotics.json",
+    "jsonUrl": "https://software-metadata.revrobotics.com/REVLib.json",
     "javaDependencies": [
         {
             "groupId": "com.revrobotics.frc",
-            "artifactId": "SparkMax-java",
-            "version": "1.5.4"
+            "artifactId": "REVLib-java",
+            "version": "2022.1.1"
         }
     ],
     "jniDependencies": [
         {
             "groupId": "com.revrobotics.frc",
-            "artifactId": "SparkMax-driver",
-            "version": "1.5.4",
+            "artifactId": "REVLib-driver",
+            "version": "2022.1.1",
             "skipInvalidPlatforms": true,
             "isJar": false,
             "validPlatforms": [
@@ -35,9 +35,9 @@
     "cppDependencies": [
         {
             "groupId": "com.revrobotics.frc",
-            "artifactId": "SparkMax-cpp",
-            "version": "1.5.4",
-            "libName": "SparkMax",
+            "artifactId": "REVLib-cpp",
+            "version": "2022.1.1",
+            "libName": "REVLib",
             "headerClassifier": "headers",
             "sharedLibrary": false,
             "skipInvalidPlatforms": true,
@@ -53,9 +53,9 @@
         },
         {
             "groupId": "com.revrobotics.frc",
-            "artifactId": "SparkMax-driver",
-            "version": "1.5.4",
-            "libName": "SparkMaxDriver",
+            "artifactId": "REVLib-driver",
+            "version": "2022.1.1",
+            "libName": "REVLibDriver",
             "headerClassifier": "headers",
             "sharedLibrary": false,
             "skipInvalidPlatforms": true,