Merge pull request #20 from ThePinkAlliance/documentation-update

New documentation

README.md

@@ -1,9 +1,64 @@
 # core
 
-This library contains some tools for handling repetive tasks like the limelight and has some unique tools like a linear interperlation table for converting values from the limelight into real distances.
+This library contains tools for the limelight, simulation, and math.
 
 ## Installation Url
 
 ```txt
 https://raw.githubusercontent.com/ThePinkAlliance/core/main/ThePinkAlliance.json
 ```
+
+## Linear Interpolation
+
+The linear interpolation class takes a list of integer pairs and will interpolate between the input (the first number in the pair) to the calculated output (the mathematically calculated second number in the pairs). Some use cases for linear interpolation could be using a table of distances give by a limelight to the real distance.
+
+### Example
+
+```java
+  /**
+  * The pairs are modeled as such (expected_input, expected_output)
+  * 
+  * In the case of the limelight example (limelight_distane, real_distance)
+  */
+  List<Pair<Integer, Integer>> points = List.of(
+      new Pair<Integer, Integer>(120, 125),
+      new Pair<Integer, Integer>(110, 115),
+      new Pair<Integer, Integer>(100, 105),
+      new Pair<Integer, Integer>(130, 135),
+      new Pair<Integer, Integer>(140, 145),
+      new Pair<Integer, Integer>(150, 155));
+  LinearInterpolationTable table = new LinearInterpolationTable(points);
+
+  double real_distance = table.interp(123); // Outputs 128 inches
+```
+
+## Spherical Coordinates
+
+The spherical coordinates class makes using the spherical coordinate system easier with some builtin utilites. Just as a quick refresher instead of using x, y, and z for units spherical coordinates use projection (r), azimuth (θ), and elevation (Φ). If your not familar with spherical coordinates theres a great article [here](https://mathinsight.org/spherical_coordinates) to help you out.
+
+### Using Spherical Coordinates
+
+```java
+  // Creating coordinates with known constants.
+  double projection = 0;
+  double azimuth = 0;
+  double elevation = 0;
+
+  SphericalCoordinates sphericalCoordinates = SphericalCoordinates.fromCartesian(projection, azimuth, elevation);
+
+  // Convert cartesian coordinates to spherical coordinates.
+  Translation3d translation3d = new Translation3d(-2, -1, -7);
+  SphericalCoordinates sphericalCoordinates = SphericalCoordinates.fromCartesian(translation3d);
+
+  // Convert spherical coordinates to catesian coordinates.
+  SphericalCoordinates coordinates = new SphericalCoordinates(7.34, -2.67, 2.83);
+  Translation3d coordinates3d = coordinates.toCartesian();
+
+  // Subtract two coordinates from eachother.
+  SphericalCoordinates desiredCoordinates = new SphericalCoordinates(1, Units.degreesToRadians(90),
+      Units.degreesToRadians(180));
+  SphericalCoordinates currentCoordinates = new SphericalCoordinates(0.5, Units.degreesToRadians(90),
+      Units.degreesToRadians(90));
+  SphericalCoordinates coordinateDifference = desiredCoordinates.subtract(currentCoordinates);
+
+```

ThePinkAlliance.json

@@ -1,7 +1,7 @@
 {
 	"fileName": "ThePinkAlliance.json",
 	"name": "ThePinkAlliance",
-	"version": "2.2.17",
+	"version": "2.3.17",
 	"uuid": "9619F7EA-7F96-4236-9D94-02338DFED572",
 	"mavenUrls": [
 		"https://jitpack.io",
@@ -13,7 +13,7 @@
 		{
 			"groupId": "com.github.ThePinkAlliance",
 			"artifactId": "core",
-			"version": "2.2.17"
+			"version": "2.3.17"
 		}
 	],
 	"jniDependencies": [],

build.gradle

@@ -2,7 +2,7 @@ apply plugin: 'java-library'
 apply plugin: 'maven-publish'
 
 group = 'com.ThePinkAlliance.core'
-version = '2.2.17'
+version = '2.3.17'
 
 sourceCompatibility = JavaVersion.VERSION_11 // java 11
 targetCompatibility = JavaVersion.VERSION_11

src/main/java/com/ThePinkAlliance/core/math/LinearInterpolationTable.java

@@ -5,6 +5,8 @@
 import java.util.List;
 import java.util.stream.Stream;
 
+import edu.wpi.first.math.Pair;
+
 /**
  * The linear interpolation takes a table with two columns and will find two
  * values in the first column, one that's smaller then the input and one
@@ -23,25 +25,28 @@
  * <a href=
  * "https://matthew-brett.github.io/teaching/linear_interpolation.html">Guide
  * from matthew brett</a>
+ * 
+ * @note The search algorithm in this utility is not optimized, average call
+ *       time 8ms.
  */
 public class LinearInterpolationTable {
-  ArrayList<Vector2d> points;
+  ArrayList<Pair<Integer, Integer>> points;
   double lastResult;
 
-  public LinearInterpolationTable(List<Vector2d> points) {
-    ArrayList<Vector2d> mutList = new ArrayList<>(points);
+  public LinearInterpolationTable(List<Pair<Integer, Integer>> points) {
+    ArrayList<Pair<Integer, Integer>> mutList = new ArrayList<>(points);
 
-    mutList.sort((a, b) -> b.x > a.x ? 1 : -1);
+    mutList.sort((a, b) -> b.getFirst() > a.getFirst() ? 1 : -1);
 
     this.points = mutList;
   }
 
   /**
    * Converts a Vector2d stream into a Vector2d list.
    */
-  public List<Vector2d> streamListVector(Stream<Vector2d> stream) {
-    Iterator<Vector2d> iterator = stream.iterator();
-    ArrayList<Vector2d> list = new ArrayList<>();
+  public List<Pair<Integer, Integer>> streamListVector(Stream<Pair<Integer, Integer>> stream) {
+    Iterator<Pair<Integer, Integer>> iterator = stream.iterator();
+    ArrayList<Pair<Integer, Integer>> list = new ArrayList<>();
 
     iterator.forEachRemaining((e) -> list.add(e));
 
@@ -64,19 +69,21 @@ public double interp(double e) {
      * don't need to worry about sorting however if we have a table with negative
      * values then sorting might become necessary.
      */
-    ArrayList<Vector2d> greaterPoints = new ArrayList<>(
+    ArrayList<Pair<Integer, Integer>> greaterPoints = new ArrayList<>(
         streamListVector(
-            points.stream().filter(v -> Math.abs(v.x) > Math.abs(e) && Math.signum(e) == Math.signum(v.x))));
-    ArrayList<Vector2d> smallerPoints = new ArrayList<>(
+            points.stream()
+                .filter(v -> Math.abs(v.getFirst()) > Math.abs(e) && Math.signum(e) == Math.signum(v.getFirst()))));
+    ArrayList<Pair<Integer, Integer>> smallerPoints = new ArrayList<>(
         streamListVector(
-            points.stream().filter(v -> Math.abs(v.x) < Math.abs(e) && Math.signum(e) == Math.signum(v.x))));
+            points.stream()
+                .filter(v -> Math.abs(v.getFirst()) < Math.abs(e) && Math.signum(e) == Math.signum(v.getFirst()))));
 
     if (greaterPoints.isEmpty() || smallerPoints.isEmpty()) {
       return Double.NaN;
     }
 
-    Vector2d vec1 = smallerPoints.get(0);
-    Vector2d vec2 = greaterPoints.get(0);
+    Pair<Integer, Integer> vec1 = smallerPoints.get(0);
+    Pair<Integer, Integer> vec2 = greaterPoints.get(0);
 
     /*
      * If vector two is undefined and vector one is defined then that means the
@@ -92,6 +99,7 @@ public double interp(double e) {
       return 0;
     }
 
-    return vec1.y + (e - vec1.x) * ((vec2.y - vec1.y) / (vec2.x - vec1.x));
+    return vec1.getSecond()
+        + (e - vec1.getFirst()) * ((vec2.getSecond() - vec1.getSecond()) / (vec2.getFirst() - vec1.getFirst()));
   }
 }

src/main/java/com/ThePinkAlliance/core/math/Projectile.java

@@ -11,6 +11,7 @@
  *
  * https://www.desmos.com/calculator/on4xzwtdwz
  */
+@Deprecated
 public class Projectile {
 
   /** this is the force of gravity on earth m/s */

src/main/java/com/ThePinkAlliance/core/math/SphericalCoordinates.java

@@ -3,20 +3,20 @@
 import edu.wpi.first.math.geometry.Translation3d;
 
 public class SphericalCoordinates {
-  private final double phi;
-  private final double theta;
-  private final double r;
-
-  public SphericalCoordinates(double r, double theta, double phi) {
-    this.phi = phi;
-    this.r = r;
-    this.theta = theta;
+  private final double elevation;
+  private final double azimuth;
+  private final double radial_distance;
+
+  public SphericalCoordinates(double distance, double azimuth, double elevation) {
+    this.elevation = elevation;
+    this.radial_distance = distance;
+    this.azimuth = azimuth;
   }
 
   public SphericalCoordinates() {
-    this.phi = 0;
-    this.r = 0;
-    this.theta = 0;
+    this.elevation = 0;
+    this.radial_distance = 0;
+    this.azimuth = 0;
   }
 
   /**
@@ -42,21 +42,32 @@ public static SphericalCoordinates fromCartesian(Translation3d translation3d) {
    * @param coordinates
    */
   public static Translation3d toCartesian(SphericalCoordinates coordinates) {
-    double x = coordinates.r * Math.sin(coordinates.phi) * Math.cos(coordinates.theta);
-    double y = coordinates.r * Math.sin(coordinates.phi) * Math.sin(coordinates.theta);
-    double z = coordinates.r * Math.cos(coordinates.phi);
+    double x = coordinates.radial_distance * Math.sin(coordinates.elevation) * Math.cos(coordinates.azimuth);
+    double y = coordinates.radial_distance * Math.sin(coordinates.elevation) * Math.sin(coordinates.azimuth);
+    double z = coordinates.radial_distance * Math.cos(coordinates.elevation);
 
     return new Translation3d(x, y, z);
   }
 
+  /**
+   * Subtracts the current coordinate by the passed coordinate.
+   */
   public SphericalCoordinates subtract(SphericalCoordinates coordinates) {
-    double r = this.r - coordinates.r;
-    double theta = this.theta - coordinates.theta;
-    double phi = this.phi - coordinates.phi;
+    double r = this.radial_distance - coordinates.radial_distance;
+    double theta = this.azimuth - coordinates.azimuth;
+    double phi = this.elevation - coordinates.elevation;
 
     return new SphericalCoordinates(r, theta, phi);
   }
 
+  /**
+   * Calculates the azimuth by subtracting the passed coordinate by the current
+   * azimuth and returns the difference.
+   */
+  public double calculateAzimuth(SphericalCoordinates coordinates) {
+    return coordinates.azimuth - this.azimuth;
+  }
+
   /**
    * Convert spherical coordinates to cartesian space.
    * 
@@ -65,16 +76,15 @@ public Translation3d toCartesian() {
     return SphericalCoordinates.toCartesian(this);
   }
 
-  public double getTheta() {
-    return theta;
+  public double getAzimuth() {
+    return azimuth;
   }
 
-  public double getPhi() {
-    return phi;
+  public double getElevation() {
+    return elevation;
   }
 
-  public double getR() {
-    return r;
+  public double getRadial_distance() {
+    return radial_distance;
   }
-
 }

src/test/java/LinearInterpolationTest.java

@@ -4,33 +4,34 @@
 
 import com.ThePinkAlliance.core.math.LinearInterpolationTable;
 import com.ThePinkAlliance.core.math.Vector2d;
+import edu.wpi.first.math.Pair;
 import org.junit.Test;
 
 public class LinearInterpolationTest {
   /**
    * NOTE: If the signs of the vectors in the list are opposites then the
    * interpolated result will be skewed.
    */
-  List<Vector2d> points = List.of(
-      new Vector2d(120, 125),
-      new Vector2d(110, 115),
-      new Vector2d(100, 105),
-      new Vector2d(130, 135),
-      new Vector2d(140, 145),
-      new Vector2d(150, 155));
-  List<Vector2d> negativePoints = List.of(
-      new Vector2d(-40, -145),
-      new Vector2d(-30, -135),
-      new Vector2d(-20,
+  List<Pair<Integer, Integer>> points = List.of(
+      new Pair<Integer, Integer>(120, 125),
+      new Pair<Integer, Integer>(110, 115),
+      new Pair<Integer, Integer>(100, 105),
+      new Pair<Integer, Integer>(130, 135),
+      new Pair<Integer, Integer>(140, 145),
+      new Pair<Integer, Integer>(150, 155));
+  List<Pair<Integer, Integer>> negativePoints = List.of(
+      new Pair<Integer, Integer>(-40, -145),
+      new Pair<Integer, Integer>(-30, -135),
+      new Pair<Integer, Integer>(-20,
           -125),
-      new Vector2d(-10, -115),
-      new Vector2d(0, 105),
-      new Vector2d(30, 135),
-      new Vector2d(40, 145),
-      new Vector2d(50, 155));
-  List<Vector2d> emptyPoints = List.of(
-      new Vector2d(0, 0),
-      new Vector2d(0, 0));
+      new Pair<Integer, Integer>(-10, -115),
+      new Pair<Integer, Integer>(0, 105),
+      new Pair<Integer, Integer>(30, 135),
+      new Pair<Integer, Integer>(40, 145),
+      new Pair<Integer, Integer>(50, 155));
+  List<Pair<Integer, Integer>> emptyPoints = List.of(
+      new Pair<Integer, Integer>(0, 0),
+      new Pair<Integer, Integer>(0, 0));
 
   LinearInterpolationTable table = new LinearInterpolationTable(points);
   LinearInterpolationTable emptyTable = new LinearInterpolationTable(emptyPoints);

src/test/java/SphericalCoordinatesTest.java

@@ -22,9 +22,17 @@ public void cartesianToSpherical() {
     Translation3d translation3d = new Translation3d(-2, -1, -7);
     SphericalCoordinates sphericalCoordinates = SphericalCoordinates.fromCartesian(translation3d);
 
-    assertEquals(7.3484, sphericalCoordinates.getR(), 0.05);
-    assertEquals(-2.6779, sphericalCoordinates.getTheta(), 0.05);
-    assertEquals(2.8323, sphericalCoordinates.getPhi(), 0.05);
+    assertEquals(7.3484, sphericalCoordinates.getRadial_distance(), 0.05);
+    assertEquals(-2.6779, sphericalCoordinates.getAzimuth(), 0.05);
+    assertEquals(2.8323, sphericalCoordinates.getElevation(), 0.05);
+  }
+
+  @Test
+  public void azimuthDifference() {
+    SphericalCoordinates sp1 = new SphericalCoordinates(10, 10, 10);
+    SphericalCoordinates sp2 = new SphericalCoordinates(10, 5, 10);
+
+    assertEquals(-5.0, sp1.calculateAzimuth(sp2), 0);
   }
 
   @Test
@@ -35,8 +43,8 @@ public void sphericalDifference() {
         Units.degreesToRadians(90));
     SphericalCoordinates coordinateDifference = desiredCoordinates.subtract(currentCoordinates);
 
-    assertEquals(0.5, coordinateDifference.getR(), 0.05);
-    assertEquals(0, coordinateDifference.getTheta(), 0.05);
-    assertEquals(1.57, coordinateDifference.getPhi(), 0.05);
+    assertEquals(0.5, coordinateDifference.getRadial_distance(), 0.05);
+    assertEquals(0, coordinateDifference.getAzimuth(), 0.05);
+    assertEquals(1.57, coordinateDifference.getElevation(), 0.05);
   }
 }