From 1920365e036b13c52d290ea8a1cfe643187b60c3 Mon Sep 17 00:00:00 2001
From: Tyler Veness <calcmogul@gmail.com>
Date: Mon, 3 Jun 2024 09:45:22 -0700
Subject: [PATCH] [wpimath] Disambiguate wpimath JNI functions

Now, each collection of JNI functions has its own class.
---
 wpimath/CMakeLists.txt                        |  16 +-
 .../main/java/edu/wpi/first/math/DARE.java    |   9 +-
 .../main/java/edu/wpi/first/math/Matrix.java  |   9 +-
 .../edu/wpi/first/math/StateSpaceUtil.java    |   6 +-
 .../java/edu/wpi/first/math/WPIMathJNI.java   | 463 ------------------
 .../first/math/controller/ArmFeedforward.java |   5 +-
 .../wpi/first/math/geometry/Ellipse2d.java    |   4 +-
 .../edu/wpi/first/math/geometry/Pose3d.java   |   6 +-
 .../wpi/first/math/jni/ArmFeedforwardJNI.java |  93 ++++
 .../java/edu/wpi/first/math/jni/DAREJNI.java  | 233 +++++++++
 .../java/edu/wpi/first/math/jni/EigenJNI.java | 114 +++++
 .../edu/wpi/first/math/jni/Ellipse2dJNI.java  |  92 ++++
 .../edu/wpi/first/math/jni/Pose3dJNI.java     | 140 ++++++
 .../wpi/first/math/jni/StateSpaceUtilJNI.java |  83 ++++
 .../wpi/first/math/jni/TrajectoryUtilJNI.java | 102 ++++
 .../first/math/trajectory/TrajectoryUtil.java |  10 +-
 ...mFeedforward.cpp => ArmFeedforwardJNI.cpp} |   6 +-
 .../jni/{WPIMathJNI_DARE.cpp => DAREJNI.cpp}  |  20 +-
 .../{WPIMathJNI_Eigen.cpp => EigenJNI.cpp}    |  18 +-
 ...MathJNI_Ellipse2d.cpp => Ellipse2dJNI.cpp} |   8 +-
 ...IMathJNI_Exceptions.cpp => Exceptions.cpp} |   2 +-
 .../{WPIMathJNI_Exceptions.h => Exceptions.h} |   0
 .../{WPIMathJNI_Pose3d.cpp => Pose3dJNI.cpp}  |  14 +-
 ...ateSpaceUtil.cpp => StateSpaceUtilJNI.cpp} |   6 +-
 ...I_Trajectory.cpp => TrajectoryUtilJNI.cpp} |  20 +-
 .../first/math/jni/ArmFeedforwardJNITest.java |  16 +
 .../DAREJNITest.java}                         |   6 +-
 .../edu/wpi/first/math/jni/EigenJNITest.java  |  16 +
 .../wpi/first/math/jni/Ellipse2dJNITest.java  |  16 +
 .../edu/wpi/first/math/jni/Pose3dJNITest.java |  16 +
 .../first/math/jni/StateSpaceUtilJNITest.java |  16 +
 .../first/math/jni/TrajectoryUtilJNITest.java |  16 +
 32 files changed, 1038 insertions(+), 543 deletions(-)
 delete mode 100644 wpimath/src/main/java/edu/wpi/first/math/WPIMathJNI.java
 create mode 100644 wpimath/src/main/java/edu/wpi/first/math/jni/ArmFeedforwardJNI.java
 create mode 100644 wpimath/src/main/java/edu/wpi/first/math/jni/DAREJNI.java
 create mode 100644 wpimath/src/main/java/edu/wpi/first/math/jni/EigenJNI.java
 create mode 100644 wpimath/src/main/java/edu/wpi/first/math/jni/Ellipse2dJNI.java
 create mode 100644 wpimath/src/main/java/edu/wpi/first/math/jni/Pose3dJNI.java
 create mode 100644 wpimath/src/main/java/edu/wpi/first/math/jni/StateSpaceUtilJNI.java
 create mode 100644 wpimath/src/main/java/edu/wpi/first/math/jni/TrajectoryUtilJNI.java
 rename wpimath/src/main/native/cpp/jni/{WPIMathJNI_ArmFeedforward.cpp => ArmFeedforwardJNI.cpp} (86%)
 rename wpimath/src/main/native/cpp/jni/{WPIMathJNI_DARE.cpp => DAREJNI.cpp} (93%)
 rename wpimath/src/main/native/cpp/jni/{WPIMathJNI_Eigen.cpp => EigenJNI.cpp} (87%)
 rename wpimath/src/main/native/cpp/jni/{WPIMathJNI_Ellipse2d.cpp => Ellipse2dJNI.cpp} (84%)
 rename wpimath/src/main/native/cpp/jni/{WPIMathJNI_Exceptions.cpp => Exceptions.cpp} (97%)
 rename wpimath/src/main/native/cpp/jni/{WPIMathJNI_Exceptions.h => Exceptions.h} (100%)
 rename wpimath/src/main/native/cpp/jni/{WPIMathJNI_Pose3d.cpp => Pose3dJNI.cpp} (89%)
 rename wpimath/src/main/native/cpp/jni/{WPIMathJNI_StateSpaceUtil.cpp => StateSpaceUtilJNI.cpp} (86%)
 rename wpimath/src/main/native/cpp/jni/{WPIMathJNI_Trajectory.cpp => TrajectoryUtilJNI.cpp} (86%)
 create mode 100644 wpimath/src/test/java/edu/wpi/first/math/jni/ArmFeedforwardJNITest.java
 rename wpimath/src/test/java/edu/wpi/first/math/{WPIMathJNITest.java => jni/DAREJNITest.java} (76%)
 create mode 100644 wpimath/src/test/java/edu/wpi/first/math/jni/EigenJNITest.java
 create mode 100644 wpimath/src/test/java/edu/wpi/first/math/jni/Ellipse2dJNITest.java
 create mode 100644 wpimath/src/test/java/edu/wpi/first/math/jni/Pose3dJNITest.java
 create mode 100644 wpimath/src/test/java/edu/wpi/first/math/jni/StateSpaceUtilJNITest.java
 create mode 100644 wpimath/src/test/java/edu/wpi/first/math/jni/TrajectoryUtilJNITest.java

diff --git a/wpimath/CMakeLists.txt b/wpimath/CMakeLists.txt
index d0984731b58..f56558410af 100644
--- a/wpimath/CMakeLists.txt
+++ b/wpimath/CMakeLists.txt
@@ -19,14 +19,14 @@ protobuf_generate_cpp(
 
 file(
     GLOB wpimath_jni_src
-    src/main/native/cpp/jni/WPIMathJNI_ArmFeedforward.cpp
-    src/main/native/cpp/jni/WPIMathJNI_DARE.cpp
-    src/main/native/cpp/jni/WPIMathJNI_Eigen.cpp
-    src/main/native/cpp/jni/WPIMathJNI_Ellipse2d.cpp
-    src/main/native/cpp/jni/WPIMathJNI_Exceptions.cpp
-    src/main/native/cpp/jni/WPIMathJNI_Pose3d.cpp
-    src/main/native/cpp/jni/WPIMathJNI_StateSpaceUtil.cpp
-    src/main/native/cpp/jni/WPIMathJNI_Trajectory.cpp
+    src/main/native/cpp/jni/ArmFeedforwardJNI.cpp
+    src/main/native/cpp/jni/DAREJNI.cpp
+    src/main/native/cpp/jni/EigenJNI.cpp
+    src/main/native/cpp/jni/Ellipse2dJNI.cpp
+    src/main/native/cpp/jni/Exceptions.cpp
+    src/main/native/cpp/jni/Pose3dJNI.cpp
+    src/main/native/cpp/jni/StateSpaceUtilJNI.cpp
+    src/main/native/cpp/jni/TrajectoryUtilJNI.cpp
 )
 
 # Java bindings
diff --git a/wpimath/src/main/java/edu/wpi/first/math/DARE.java b/wpimath/src/main/java/edu/wpi/first/math/DARE.java
index 44c488dc526..504a863279b 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/DARE.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/DARE.java
@@ -4,6 +4,7 @@
 
 package edu.wpi.first.math;
 
+import edu.wpi.first.math.jni.DAREJNI;
 import org.ejml.simple.SimpleMatrix;
 
 /** DARE solver utility functions. */
@@ -43,7 +44,7 @@ public static <States extends Num, Inputs extends Num> Matrix<States, States> da
       Matrix<States, States> Q,
       Matrix<Inputs, Inputs> R) {
     var S = new Matrix<States, States>(new SimpleMatrix(A.getNumRows(), A.getNumCols()));
-    WPIMathJNI.dareDetailABQR(
+    DAREJNI.dareDetailABQR(
         A.getStorage().getDDRM().getData(),
         B.getStorage().getDDRM().getData(),
         Q.getStorage().getDDRM().getData(),
@@ -121,7 +122,7 @@ public static <States extends Num, Inputs extends Num> Matrix<States, States> da
       Matrix<Inputs, Inputs> R,
       Matrix<States, Inputs> N) {
     var S = new Matrix<States, States>(new SimpleMatrix(A.getNumRows(), A.getNumCols()));
-    WPIMathJNI.dareDetailABQRN(
+    DAREJNI.dareDetailABQRN(
         A.getStorage().getDDRM().getData(),
         B.getStorage().getDDRM().getData(),
         Q.getStorage().getDDRM().getData(),
@@ -156,7 +157,7 @@ public static <States extends Num, Inputs extends Num> Matrix<States, States> da
       Matrix<States, States> Q,
       Matrix<Inputs, Inputs> R) {
     var S = new Matrix<States, States>(new SimpleMatrix(A.getNumRows(), A.getNumCols()));
-    WPIMathJNI.dareABQR(
+    DAREJNI.dareABQR(
         A.getStorage().getDDRM().getData(),
         B.getStorage().getDDRM().getData(),
         Q.getStorage().getDDRM().getData(),
@@ -226,7 +227,7 @@ public static <States extends Num, Inputs extends Num> Matrix<States, States> da
       Matrix<Inputs, Inputs> R,
       Matrix<States, Inputs> N) {
     var S = new Matrix<States, States>(new SimpleMatrix(A.getNumRows(), A.getNumCols()));
-    WPIMathJNI.dareABQRN(
+    DAREJNI.dareABQRN(
         A.getStorage().getDDRM().getData(),
         B.getStorage().getDDRM().getData(),
         Q.getStorage().getDDRM().getData(),
diff --git a/wpimath/src/main/java/edu/wpi/first/math/Matrix.java b/wpimath/src/main/java/edu/wpi/first/math/Matrix.java
index cd77283649d..2a7f0aa1803 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/Matrix.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/Matrix.java
@@ -4,6 +4,7 @@
 
 package edu.wpi.first.math;
 
+import edu.wpi.first.math.jni.EigenJNI;
 import edu.wpi.first.math.numbers.N1;
 import java.util.Objects;
 import org.ejml.MatrixDimensionException;
@@ -360,7 +361,7 @@ public final <C2 extends Num> Matrix<C, C2> solve(Matrix<R, C2> b) {
   public final <R2 extends Num, C2 extends Num> Matrix<C, C2> solveFullPivHouseholderQr(
       Matrix<R2, C2> other) {
     Matrix<C, C2> solution = new Matrix<>(new SimpleMatrix(this.getNumCols(), other.getNumCols()));
-    WPIMathJNI.solveFullPivHouseholderQr(
+    EigenJNI.solveFullPivHouseholderQr(
         this.getData(),
         this.getNumRows(),
         this.getNumCols(),
@@ -387,7 +388,7 @@ public final Matrix<R, C> exp() {
               + this.getNumCols());
     }
     Matrix<R, C> toReturn = new Matrix<>(new SimpleMatrix(this.getNumRows(), this.getNumCols()));
-    WPIMathJNI.exp(
+    EigenJNI.exp(
         this.m_storage.getDDRM().getData(),
         this.getNumRows(),
         toReturn.m_storage.getDDRM().getData());
@@ -411,7 +412,7 @@ public final Matrix<R, C> pow(double exponent) {
               + this.getNumCols());
     }
     Matrix<R, C> toReturn = new Matrix<>(new SimpleMatrix(this.getNumRows(), this.getNumCols()));
-    WPIMathJNI.pow(
+    EigenJNI.pow(
         this.m_storage.getDDRM().getData(),
         this.getNumRows(),
         exponent,
@@ -709,7 +710,7 @@ public boolean isEqual(Matrix<?, ?> other, double tolerance) {
    * @param lowerTriangular Whether this matrix is lower triangular.
    */
   public void rankUpdate(Matrix<R, N1> v, double sigma, boolean lowerTriangular) {
-    WPIMathJNI.rankUpdate(this.getData(), this.getNumRows(), v.getData(), sigma, lowerTriangular);
+    EigenJNI.rankUpdate(this.getData(), this.getNumRows(), v.getData(), sigma, lowerTriangular);
   }
 
   @Override
diff --git a/wpimath/src/main/java/edu/wpi/first/math/StateSpaceUtil.java b/wpimath/src/main/java/edu/wpi/first/math/StateSpaceUtil.java
index 4e003747693..461d9a6f080 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/StateSpaceUtil.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/StateSpaceUtil.java
@@ -5,6 +5,7 @@
 package edu.wpi.first.math;
 
 import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.jni.StateSpaceUtilJNI;
 import edu.wpi.first.math.numbers.N1;
 import edu.wpi.first.math.numbers.N3;
 import edu.wpi.first.math.numbers.N4;
@@ -102,7 +103,8 @@ public static <Elements extends Num> Matrix<Elements, Elements> makeCostMatrix(
    */
   public static <States extends Num, Inputs extends Num> boolean isStabilizable(
       Matrix<States, States> A, Matrix<States, Inputs> B) {
-    return WPIMathJNI.isStabilizable(A.getNumRows(), B.getNumCols(), A.getData(), B.getData());
+    return StateSpaceUtilJNI.isStabilizable(
+        A.getNumRows(), B.getNumCols(), A.getData(), B.getData());
   }
 
   /**
@@ -120,7 +122,7 @@ public static <States extends Num, Inputs extends Num> boolean isStabilizable(
    */
   public static <States extends Num, Outputs extends Num> boolean isDetectable(
       Matrix<States, States> A, Matrix<Outputs, States> C) {
-    return WPIMathJNI.isStabilizable(
+    return StateSpaceUtilJNI.isStabilizable(
         A.getNumRows(), C.getNumRows(), A.transpose().getData(), C.transpose().getData());
   }
 
diff --git a/wpimath/src/main/java/edu/wpi/first/math/WPIMathJNI.java b/wpimath/src/main/java/edu/wpi/first/math/WPIMathJNI.java
deleted file mode 100644
index 10a9147c7d2..00000000000
--- a/wpimath/src/main/java/edu/wpi/first/math/WPIMathJNI.java
+++ /dev/null
@@ -1,463 +0,0 @@
-// 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 edu.wpi.first.math;
-
-import edu.wpi.first.util.RuntimeLoader;
-import java.io.IOException;
-import java.util.concurrent.atomic.AtomicBoolean;
-
-/** WPIMath JNI. */
-public final class WPIMathJNI {
-  static boolean libraryLoaded = false;
-
-  static {
-    if (Helper.getExtractOnStaticLoad()) {
-      try {
-        RuntimeLoader.loadLibrary("wpimathjni");
-      } catch (Exception ex) {
-        ex.printStackTrace();
-        System.exit(1);
-      }
-      libraryLoaded = true;
-    }
-  }
-
-  /**
-   * Force load the library.
-   *
-   * @throws IOException If the library could not be loaded or found.
-   */
-  public static synchronized void forceLoad() throws IOException {
-    if (libraryLoaded) {
-      return;
-    }
-    RuntimeLoader.loadLibrary("wpimathjni");
-    libraryLoaded = true;
-  }
-
-  // ArmFeedforward wrappers
-
-  /**
-   * Obtain a feedforward voltage from a single jointed arm feedforward object.
-   *
-   * <p>Constructs an ArmFeedforward object and runs its currentVelocity and nextVelocity overload
-   *
-   * @param ks The ArmFeedforward's static gain in volts.
-   * @param kv The ArmFeedforward's velocity gain in volt seconds per radian.
-   * @param ka The ArmFeedforward's acceleration gain in volt seconds² per radian.
-   * @param kg The ArmFeedforward's gravity gain in volts.
-   * @param currentAngle The current angle in the calculation in radians.
-   * @param currentVelocity The current velocity in the calculation in radians per second.
-   * @param nextVelocity The next velocity in the calculation in radians per second.
-   * @param dt The time between velocity setpoints in seconds.
-   * @return The calculated feedforward in volts.
-   */
-  public static native double calculate(
-      double ks,
-      double kv,
-      double ka,
-      double kg,
-      double currentAngle,
-      double currentVelocity,
-      double nextVelocity,
-      double dt);
-
-  // DARE wrappers
-
-  /**
-   * Computes the unique stabilizing solution X to the discrete-time algebraic Riccati equation.
-   *
-   * <p>AᵀXA − X − AᵀXB(BᵀXB + R)⁻¹BᵀXA + Q = 0
-   *
-   * <p>This internal function skips expensive precondition checks for increased performance. The
-   * solver may hang if any of the following occur:
-   *
-   * <ul>
-   *   <li>Q isn't symmetric positive semidefinite
-   *   <li>R isn't symmetric positive definite
-   *   <li>The (A, B) pair isn't stabilizable
-   *   <li>The (A, C) pair where Q = CᵀC isn't detectable
-   * </ul>
-   *
-   * <p>Only use this function if you're sure the preconditions are met. Solves the discrete
-   * alegebraic Riccati equation.
-   *
-   * @param A Array containing elements of A in row-major order.
-   * @param B Array containing elements of B in row-major order.
-   * @param Q Array containing elements of Q in row-major order.
-   * @param R Array containing elements of R in row-major order.
-   * @param states Number of states in A matrix.
-   * @param inputs Number of inputs in B matrix.
-   * @param S Array storage for DARE solution.
-   */
-  public static native void dareDetailABQR(
-      double[] A, double[] B, double[] Q, double[] R, int states, int inputs, double[] S);
-
-  /**
-   * Computes the unique stabilizing solution X to the discrete-time algebraic Riccati equation.
-   *
-   * <p>AᵀXA − X − (AᵀXB + N)(BᵀXB + R)⁻¹(BᵀXA + Nᵀ) + Q = 0
-   *
-   * <p>This overload of the DARE is useful for finding the control law uₖ that minimizes the
-   * following cost function subject to xₖ₊₁ = Axₖ + Buₖ.
-   *
-   * <pre>
-   *     ∞ [xₖ]ᵀ[Q  N][xₖ]
-   * J = Σ [uₖ] [Nᵀ R][uₖ] ΔT
-   *    k=0
-   * </pre>
-   *
-   * <p>This is a more general form of the following. The linear-quadratic regulator is the feedback
-   * control law uₖ that minimizes the following cost function subject to xₖ₊₁ = Axₖ + Buₖ:
-   *
-   * <pre>
-   *     ∞
-   * J = Σ (xₖᵀQxₖ + uₖᵀRuₖ) ΔT
-   *    k=0
-   * </pre>
-   *
-   * <p>This can be refactored as:
-   *
-   * <pre>
-   *     ∞ [xₖ]ᵀ[Q 0][xₖ]
-   * J = Σ [uₖ] [0 R][uₖ] ΔT
-   *    k=0
-   * </pre>
-   *
-   * <p>This internal function skips expensive precondition checks for increased performance. The
-   * solver may hang if any of the following occur:
-   *
-   * <ul>
-   *   <li>Q − NR⁻¹Nᵀ isn't symmetric positive semidefinite
-   *   <li>R isn't symmetric positive definite
-   *   <li>The (A − BR⁻¹Nᵀ, B) pair isn't stabilizable
-   *   <li>The (A, C) pair where Q = CᵀC isn't detectable
-   * </ul>
-   *
-   * <p>Only use this function if you're sure the preconditions are met.
-   *
-   * @param A Array containing elements of A in row-major order.
-   * @param B Array containing elements of B in row-major order.
-   * @param Q Array containing elements of Q in row-major order.
-   * @param R Array containing elements of R in row-major order.
-   * @param N Array containing elements of N in row-major order.
-   * @param states Number of states in A matrix.
-   * @param inputs Number of inputs in B matrix.
-   * @param S Array storage for DARE solution.
-   */
-  public static native void dareDetailABQRN(
-      double[] A,
-      double[] B,
-      double[] Q,
-      double[] R,
-      double[] N,
-      int states,
-      int inputs,
-      double[] S);
-
-  /**
-   * Computes the unique stabilizing solution X to the discrete-time algebraic Riccati equation.
-   *
-   * <p>AᵀXA − X − AᵀXB(BᵀXB + R)⁻¹BᵀXA + Q = 0
-   *
-   * @param A Array containing elements of A in row-major order.
-   * @param B Array containing elements of B in row-major order.
-   * @param Q Array containing elements of Q in row-major order.
-   * @param R Array containing elements of R in row-major order.
-   * @param states Number of states in A matrix.
-   * @param inputs Number of inputs in B matrix.
-   * @param S Array storage for DARE solution.
-   * @throws IllegalArgumentException if Q isn't symmetric positive semidefinite.
-   * @throws IllegalArgumentException if R isn't symmetric positive definite.
-   * @throws IllegalArgumentException if the (A, B) pair isn't stabilizable.
-   * @throws IllegalArgumentException if the (A, C) pair where Q = CᵀC isn't detectable.
-   */
-  public static native void dareABQR(
-      double[] A, double[] B, double[] Q, double[] R, int states, int inputs, double[] S);
-
-  /**
-   * Computes the unique stabilizing solution X to the discrete-time algebraic Riccati equation.
-   *
-   * <p>AᵀXA − X − (AᵀXB + N)(BᵀXB + R)⁻¹(BᵀXA + Nᵀ) + Q = 0
-   *
-   * <p>This overload of the DARE is useful for finding the control law uₖ that minimizes the
-   * following cost function subject to xₖ₊₁ = Axₖ + Buₖ.
-   *
-   * <pre>
-   *     ∞ [xₖ]ᵀ[Q  N][xₖ]
-   * J = Σ [uₖ] [Nᵀ R][uₖ] ΔT
-   *    k=0
-   * </pre>
-   *
-   * <p>This is a more general form of the following. The linear-quadratic regulator is the feedback
-   * control law uₖ that minimizes the following cost function subject to xₖ₊₁ = Axₖ + Buₖ:
-   *
-   * <pre>
-   *     ∞
-   * J = Σ (xₖᵀQxₖ + uₖᵀRuₖ) ΔT
-   *    k=0
-   * </pre>
-   *
-   * <p>This can be refactored as:
-   *
-   * <pre>
-   *     ∞ [xₖ]ᵀ[Q 0][xₖ]
-   * J = Σ [uₖ] [0 R][uₖ] ΔT
-   *    k=0
-   * </pre>
-   *
-   * @param A Array containing elements of A in row-major order.
-   * @param B Array containing elements of B in row-major order.
-   * @param Q Array containing elements of Q in row-major order.
-   * @param R Array containing elements of R in row-major order.
-   * @param N Array containing elements of N in row-major order.
-   * @param states Number of states in A matrix.
-   * @param inputs Number of inputs in B matrix.
-   * @param S Array storage for DARE solution.
-   * @throws IllegalArgumentException if Q − NR⁻¹Nᵀ isn't symmetric positive semidefinite.
-   * @throws IllegalArgumentException if R isn't symmetric positive definite.
-   * @throws IllegalArgumentException if the (A − BR⁻¹Nᵀ, B) pair isn't stabilizable.
-   * @throws IllegalArgumentException if the (A, C) pair where Q = CᵀC isn't detectable.
-   */
-  public static native void dareABQRN(
-      double[] A,
-      double[] B,
-      double[] Q,
-      double[] R,
-      double[] N,
-      int states,
-      int inputs,
-      double[] S);
-
-  // Eigen wrappers
-
-  /**
-   * Computes the matrix exp.
-   *
-   * @param src Array of elements of the matrix to be exponentiated.
-   * @param rows How many rows there are.
-   * @param dst Array where the result will be stored.
-   */
-  public static native void exp(double[] src, int rows, double[] dst);
-
-  /**
-   * Computes the matrix pow.
-   *
-   * @param src Array of elements of the matrix to be raised to a power.
-   * @param rows How many rows there are.
-   * @param exponent The exponent.
-   * @param dst Array where the result will be stored.
-   */
-  public static native void pow(double[] src, int rows, double exponent, double[] dst);
-
-  /**
-   * Performs an inplace rank one update (or downdate) of an upper triangular Cholesky decomposition
-   * matrix.
-   *
-   * @param mat Array of elements of the matrix to be updated.
-   * @param lowerTriangular Whether mat is lower triangular.
-   * @param rows How many rows there are.
-   * @param vec Vector to use for the rank update.
-   * @param sigma Sigma value to use for the rank update.
-   */
-  public static native void rankUpdate(
-      double[] mat, int rows, double[] vec, double sigma, boolean lowerTriangular);
-
-  /**
-   * Solves the least-squares problem Ax=B using a QR decomposition with full pivoting.
-   *
-   * @param A Array of elements of the A matrix.
-   * @param Arows Number of rows of the A matrix.
-   * @param Acols Number of rows of the A matrix.
-   * @param B Array of elements of the B matrix.
-   * @param Brows Number of rows of the B matrix.
-   * @param Bcols Number of rows of the B matrix.
-   * @param dst Array to store solution in. If A is m-n and B is m-p, dst is n-p.
-   */
-  public static native void solveFullPivHouseholderQr(
-      double[] A, int Arows, int Acols, double[] B, int Brows, int Bcols, double[] dst);
-
-  // Ellipse2d wrappers
-
-  /**
-   * Returns the nearest point that is contained within the ellipse.
-   *
-   * <p>Constructs an Ellipse2d object and runs its FindNearestPoint() method.
-   *
-   * @param centerX The x coordinate of the center of the ellipse in meters.
-   * @param centerY The y coordinate of the center of the ellipse in meters.
-   * @param centerHeading The ellipse's rotation in radians.
-   * @param xSemiAxis The x semi-axis in meters.
-   * @param ySemiAxis The y semi-axis in meters.
-   * @param pointX The x coordinate of the point that this will find the nearest point to.
-   * @param pointY The y coordinate of the point that this will find the nearest point to.
-   * @param nearestPoint Array to store nearest point into.
-   */
-  public static native void ellipse2dFindNearestPoint(
-      double centerX,
-      double centerY,
-      double centerHeading,
-      double xSemiAxis,
-      double ySemiAxis,
-      double pointX,
-      double pointY,
-      double[] nearestPoint);
-
-  // Pose3d wrappers
-
-  /**
-   * Obtain a Pose3d from a (constant curvature) velocity.
-   *
-   * <p>The double array returned is of the form [dx, dy, dz, qx, qy, qz].
-   *
-   * @param poseX The pose's translational X component.
-   * @param poseY The pose's translational Y component.
-   * @param poseZ The pose's translational Z component.
-   * @param poseQw The pose quaternion's W component.
-   * @param poseQx The pose quaternion's X component.
-   * @param poseQy The pose quaternion's Y component.
-   * @param poseQz The pose quaternion's Z component.
-   * @param twistDx The twist's dx value.
-   * @param twistDy The twist's dy value.
-   * @param twistDz The twist's dz value.
-   * @param twistRx The twist's rx value.
-   * @param twistRy The twist's ry value.
-   * @param twistRz The twist's rz value.
-   * @return The new pose as a double array.
-   */
-  public static native double[] expPose3d(
-      double poseX,
-      double poseY,
-      double poseZ,
-      double poseQw,
-      double poseQx,
-      double poseQy,
-      double poseQz,
-      double twistDx,
-      double twistDy,
-      double twistDz,
-      double twistRx,
-      double twistRy,
-      double twistRz);
-
-  /**
-   * Returns a Twist3d that maps the starting pose to the end pose.
-   *
-   * <p>The double array returned is of the form [dx, dy, dz, rx, ry, rz].
-   *
-   * @param startX The starting pose's translational X component.
-   * @param startY The starting pose's translational Y component.
-   * @param startZ The starting pose's translational Z component.
-   * @param startQw The starting pose quaternion's W component.
-   * @param startQx The starting pose quaternion's X component.
-   * @param startQy The starting pose quaternion's Y component.
-   * @param startQz The starting pose quaternion's Z component.
-   * @param endX The ending pose's translational X component.
-   * @param endY The ending pose's translational Y component.
-   * @param endZ The ending pose's translational Z component.
-   * @param endQw The ending pose quaternion's W component.
-   * @param endQx The ending pose quaternion's X component.
-   * @param endQy The ending pose quaternion's Y component.
-   * @param endQz The ending pose quaternion's Z component.
-   * @return The twist that maps start to end as a double array.
-   */
-  public static native double[] logPose3d(
-      double startX,
-      double startY,
-      double startZ,
-      double startQw,
-      double startQx,
-      double startQy,
-      double startQz,
-      double endX,
-      double endY,
-      double endZ,
-      double endQw,
-      double endQx,
-      double endQy,
-      double endQz);
-
-  // StateSpaceUtil wrappers
-
-  /**
-   * Returns true if (A, B) is a stabilizable pair.
-   *
-   * <p>(A, B) is stabilizable if and only if the uncontrollable eigenvalues of A, if any, have
-   * absolute values less than one, where an eigenvalue is uncontrollable if rank(lambda * I - A, B)
-   * &lt; n where n is the number of states.
-   *
-   * @param states the number of states of the system.
-   * @param inputs the number of inputs to the system.
-   * @param A System matrix.
-   * @param B Input matrix.
-   * @return If the system is stabilizable.
-   */
-  public static native boolean isStabilizable(int states, int inputs, double[] A, double[] B);
-
-  // Trajectory wrappers
-
-  /**
-   * Loads a Pathweaver JSON.
-   *
-   * @param path The path to the JSON.
-   * @return A double array with the trajectory states from the JSON.
-   * @throws IOException if the JSON could not be read.
-   */
-  public static native double[] fromPathweaverJson(String path) throws IOException;
-
-  /**
-   * Converts a trajectory into a Pathweaver JSON and saves it.
-   *
-   * @param elements The elements of the trajectory.
-   * @param path The location to save the JSON to.
-   * @throws IOException if the JSON could not be written.
-   */
-  public static native void toPathweaverJson(double[] elements, String path) throws IOException;
-
-  /**
-   * Deserializes a trajectory JSON into a double[] of trajectory elements.
-   *
-   * @param json The JSON containing the serialized trajectory.
-   * @return A double array with the trajectory states.
-   */
-  public static native double[] deserializeTrajectory(String json);
-
-  /**
-   * Serializes the trajectory into a JSON string.
-   *
-   * @param elements The elements of the trajectory.
-   * @return A JSON containing the serialized trajectory.
-   */
-  public static native String serializeTrajectory(double[] elements);
-
-  /** Sets whether JNI should be loaded in the static block. */
-  public static class Helper {
-    private static AtomicBoolean extractOnStaticLoad = new AtomicBoolean(true);
-
-    /**
-     * Returns true if the JNI should be loaded in the static block.
-     *
-     * @return True if the JNI should be loaded in the static block.
-     */
-    public static boolean getExtractOnStaticLoad() {
-      return extractOnStaticLoad.get();
-    }
-
-    /**
-     * Sets whether the JNI should be loaded in the static block.
-     *
-     * @param load Whether the JNI should be loaded in the static block.
-     */
-    public static void setExtractOnStaticLoad(boolean load) {
-      extractOnStaticLoad.set(load);
-    }
-
-    /** Utility class. */
-    private Helper() {}
-  }
-
-  /** Utility class. */
-  private WPIMathJNI() {}
-}
diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/ArmFeedforward.java b/wpimath/src/main/java/edu/wpi/first/math/controller/ArmFeedforward.java
index 0d479418063..32464ece107 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/controller/ArmFeedforward.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/controller/ArmFeedforward.java
@@ -4,9 +4,9 @@
 
 package edu.wpi.first.math.controller;
 
-import edu.wpi.first.math.WPIMathJNI;
 import edu.wpi.first.math.controller.proto.ArmFeedforwardProto;
 import edu.wpi.first.math.controller.struct.ArmFeedforwardStruct;
+import edu.wpi.first.math.jni.ArmFeedforwardJNI;
 import edu.wpi.first.util.protobuf.ProtobufSerializable;
 import edu.wpi.first.util.struct.StructSerializable;
 
@@ -114,7 +114,8 @@ public double calculate(double positionRadians, double velocity) {
    */
   public double calculate(
       double currentAngle, double currentVelocity, double nextVelocity, double dt) {
-    return WPIMathJNI.calculate(ks, kv, ka, kg, currentAngle, currentVelocity, nextVelocity, dt);
+    return ArmFeedforwardJNI.calculate(
+        ks, kv, ka, kg, currentAngle, currentVelocity, nextVelocity, dt);
   }
 
   // Rearranging the main equation from the calculate() method yields the
diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Ellipse2d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Ellipse2d.java
index e52464a30e6..499dae86545 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Ellipse2d.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Ellipse2d.java
@@ -5,9 +5,9 @@
 package edu.wpi.first.math.geometry;
 
 import edu.wpi.first.math.Pair;
-import edu.wpi.first.math.WPIMathJNI;
 import edu.wpi.first.math.geometry.proto.Ellipse2dProto;
 import edu.wpi.first.math.geometry.struct.Ellipse2dStruct;
+import edu.wpi.first.math.jni.Ellipse2dJNI;
 import edu.wpi.first.util.protobuf.ProtobufSerializable;
 import edu.wpi.first.util.struct.StructSerializable;
 import java.util.Objects;
@@ -172,7 +172,7 @@ public Translation2d findNearestPoint(Translation2d point) {
 
     // Find nearest point
     var nearestPoint = new double[2];
-    WPIMathJNI.ellipse2dFindNearestPoint(
+    Ellipse2dJNI.findNearestPoint(
         m_center.getX(),
         m_center.getY(),
         m_center.getRotation().getRadians(),
diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose3d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose3d.java
index 9cebf0882d0..dcd5ec5ba03 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose3d.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose3d.java
@@ -8,10 +8,10 @@
 import com.fasterxml.jackson.annotation.JsonCreator;
 import com.fasterxml.jackson.annotation.JsonIgnoreProperties;
 import com.fasterxml.jackson.annotation.JsonProperty;
-import edu.wpi.first.math.WPIMathJNI;
 import edu.wpi.first.math.geometry.proto.Pose3dProto;
 import edu.wpi.first.math.geometry.struct.Pose3dStruct;
 import edu.wpi.first.math.interpolation.Interpolatable;
+import edu.wpi.first.math.jni.Pose3dJNI;
 import edu.wpi.first.util.protobuf.ProtobufSerializable;
 import edu.wpi.first.util.struct.StructSerializable;
 import java.util.Objects;
@@ -224,7 +224,7 @@ public Pose3d relativeTo(Pose3d other) {
   public Pose3d exp(Twist3d twist) {
     var quaternion = this.getRotation().getQuaternion();
     double[] resultArray =
-        WPIMathJNI.expPose3d(
+        Pose3dJNI.exp(
             this.getX(),
             this.getY(),
             this.getZ(),
@@ -257,7 +257,7 @@ public Twist3d log(Pose3d end) {
     var thisQuaternion = this.getRotation().getQuaternion();
     var endQuaternion = end.getRotation().getQuaternion();
     double[] resultArray =
-        WPIMathJNI.logPose3d(
+        Pose3dJNI.log(
             this.getX(),
             this.getY(),
             this.getZ(),
diff --git a/wpimath/src/main/java/edu/wpi/first/math/jni/ArmFeedforwardJNI.java b/wpimath/src/main/java/edu/wpi/first/math/jni/ArmFeedforwardJNI.java
new file mode 100644
index 00000000000..22796cf2464
--- /dev/null
+++ b/wpimath/src/main/java/edu/wpi/first/math/jni/ArmFeedforwardJNI.java
@@ -0,0 +1,93 @@
+// 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 edu.wpi.first.math.jni;
+
+import edu.wpi.first.util.RuntimeLoader;
+import java.io.IOException;
+import java.util.concurrent.atomic.AtomicBoolean;
+
+/** ArmFeedforward JNI. */
+public final class ArmFeedforwardJNI {
+  static boolean libraryLoaded = false;
+
+  static {
+    if (Helper.getExtractOnStaticLoad()) {
+      try {
+        RuntimeLoader.loadLibrary("wpimathjni");
+      } catch (Exception ex) {
+        ex.printStackTrace();
+        System.exit(1);
+      }
+      libraryLoaded = true;
+    }
+  }
+
+  /**
+   * Force load the library.
+   *
+   * @throws IOException If the library could not be loaded or found.
+   */
+  public static synchronized void forceLoad() throws IOException {
+    if (libraryLoaded) {
+      return;
+    }
+    RuntimeLoader.loadLibrary("wpimathjni");
+    libraryLoaded = true;
+  }
+
+  /**
+   * Obtain a feedforward voltage from a single jointed arm feedforward object.
+   *
+   * <p>Constructs an ArmFeedforward object and runs its currentVelocity and nextVelocity overload
+   *
+   * @param ks The ArmFeedforward's static gain in volts.
+   * @param kv The ArmFeedforward's velocity gain in volt seconds per radian.
+   * @param ka The ArmFeedforward's acceleration gain in volt seconds² per radian.
+   * @param kg The ArmFeedforward's gravity gain in volts.
+   * @param currentAngle The current angle in the calculation in radians.
+   * @param currentVelocity The current velocity in the calculation in radians per second.
+   * @param nextVelocity The next velocity in the calculation in radians per second.
+   * @param dt The time between velocity setpoints in seconds.
+   * @return The calculated feedforward in volts.
+   */
+  public static native double calculate(
+      double ks,
+      double kv,
+      double ka,
+      double kg,
+      double currentAngle,
+      double currentVelocity,
+      double nextVelocity,
+      double dt);
+
+  /** Sets whether JNI should be loaded in the static block. */
+  public static class Helper {
+    private static AtomicBoolean extractOnStaticLoad = new AtomicBoolean(true);
+
+    /**
+     * Returns true if the JNI should be loaded in the static block.
+     *
+     * @return True if the JNI should be loaded in the static block.
+     */
+    public static boolean getExtractOnStaticLoad() {
+      return extractOnStaticLoad.get();
+    }
+
+    /**
+     * Sets whether the JNI should be loaded in the static block.
+     *
+     * @param load Whether the JNI should be loaded in the static block.
+     */
+    public static void setExtractOnStaticLoad(boolean load) {
+      extractOnStaticLoad.set(load);
+    }
+
+    /** Utility class. */
+    private Helper() {}
+  }
+
+  /** Utility class. */
+  private ArmFeedforwardJNI() {}
+}
diff --git a/wpimath/src/main/java/edu/wpi/first/math/jni/DAREJNI.java b/wpimath/src/main/java/edu/wpi/first/math/jni/DAREJNI.java
new file mode 100644
index 00000000000..8dcdf0e72a7
--- /dev/null
+++ b/wpimath/src/main/java/edu/wpi/first/math/jni/DAREJNI.java
@@ -0,0 +1,233 @@
+// 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 edu.wpi.first.math.jni;
+
+import edu.wpi.first.util.RuntimeLoader;
+import java.io.IOException;
+import java.util.concurrent.atomic.AtomicBoolean;
+
+/** DARE JNI. */
+public final class DAREJNI {
+  static boolean libraryLoaded = false;
+
+  static {
+    if (Helper.getExtractOnStaticLoad()) {
+      try {
+        RuntimeLoader.loadLibrary("wpimathjni");
+      } catch (Exception ex) {
+        ex.printStackTrace();
+        System.exit(1);
+      }
+      libraryLoaded = true;
+    }
+  }
+
+  /**
+   * Force load the library.
+   *
+   * @throws IOException If the library could not be loaded or found.
+   */
+  public static synchronized void forceLoad() throws IOException {
+    if (libraryLoaded) {
+      return;
+    }
+    RuntimeLoader.loadLibrary("wpimathjni");
+    libraryLoaded = true;
+  }
+
+  /**
+   * Computes the unique stabilizing solution X to the discrete-time algebraic Riccati equation.
+   *
+   * <p>AᵀXA − X − AᵀXB(BᵀXB + R)⁻¹BᵀXA + Q = 0
+   *
+   * <p>This internal function skips expensive precondition checks for increased performance. The
+   * solver may hang if any of the following occur:
+   *
+   * <ul>
+   *   <li>Q isn't symmetric positive semidefinite
+   *   <li>R isn't symmetric positive definite
+   *   <li>The (A, B) pair isn't stabilizable
+   *   <li>The (A, C) pair where Q = CᵀC isn't detectable
+   * </ul>
+   *
+   * <p>Only use this function if you're sure the preconditions are met. Solves the discrete
+   * alegebraic Riccati equation.
+   *
+   * @param A Array containing elements of A in row-major order.
+   * @param B Array containing elements of B in row-major order.
+   * @param Q Array containing elements of Q in row-major order.
+   * @param R Array containing elements of R in row-major order.
+   * @param states Number of states in A matrix.
+   * @param inputs Number of inputs in B matrix.
+   * @param S Array storage for DARE solution.
+   */
+  public static native void dareDetailABQR(
+      double[] A, double[] B, double[] Q, double[] R, int states, int inputs, double[] S);
+
+  /**
+   * Computes the unique stabilizing solution X to the discrete-time algebraic Riccati equation.
+   *
+   * <p>AᵀXA − X − (AᵀXB + N)(BᵀXB + R)⁻¹(BᵀXA + Nᵀ) + Q = 0
+   *
+   * <p>This overload of the DARE is useful for finding the control law uₖ that minimizes the
+   * following cost function subject to xₖ₊₁ = Axₖ + Buₖ.
+   *
+   * <pre>
+   *     ∞ [xₖ]ᵀ[Q  N][xₖ]
+   * J = Σ [uₖ] [Nᵀ R][uₖ] ΔT
+   *    k=0
+   * </pre>
+   *
+   * <p>This is a more general form of the following. The linear-quadratic regulator is the feedback
+   * control law uₖ that minimizes the following cost function subject to xₖ₊₁ = Axₖ + Buₖ:
+   *
+   * <pre>
+   *     ∞
+   * J = Σ (xₖᵀQxₖ + uₖᵀRuₖ) ΔT
+   *    k=0
+   * </pre>
+   *
+   * <p>This can be refactored as:
+   *
+   * <pre>
+   *     ∞ [xₖ]ᵀ[Q 0][xₖ]
+   * J = Σ [uₖ] [0 R][uₖ] ΔT
+   *    k=0
+   * </pre>
+   *
+   * <p>This internal function skips expensive precondition checks for increased performance. The
+   * solver may hang if any of the following occur:
+   *
+   * <ul>
+   *   <li>Q − NR⁻¹Nᵀ isn't symmetric positive semidefinite
+   *   <li>R isn't symmetric positive definite
+   *   <li>The (A − BR⁻¹Nᵀ, B) pair isn't stabilizable
+   *   <li>The (A, C) pair where Q = CᵀC isn't detectable
+   * </ul>
+   *
+   * <p>Only use this function if you're sure the preconditions are met.
+   *
+   * @param A Array containing elements of A in row-major order.
+   * @param B Array containing elements of B in row-major order.
+   * @param Q Array containing elements of Q in row-major order.
+   * @param R Array containing elements of R in row-major order.
+   * @param N Array containing elements of N in row-major order.
+   * @param states Number of states in A matrix.
+   * @param inputs Number of inputs in B matrix.
+   * @param S Array storage for DARE solution.
+   */
+  public static native void dareDetailABQRN(
+      double[] A,
+      double[] B,
+      double[] Q,
+      double[] R,
+      double[] N,
+      int states,
+      int inputs,
+      double[] S);
+
+  /**
+   * Computes the unique stabilizing solution X to the discrete-time algebraic Riccati equation.
+   *
+   * <p>AᵀXA − X − AᵀXB(BᵀXB + R)⁻¹BᵀXA + Q = 0
+   *
+   * @param A Array containing elements of A in row-major order.
+   * @param B Array containing elements of B in row-major order.
+   * @param Q Array containing elements of Q in row-major order.
+   * @param R Array containing elements of R in row-major order.
+   * @param states Number of states in A matrix.
+   * @param inputs Number of inputs in B matrix.
+   * @param S Array storage for DARE solution.
+   * @throws IllegalArgumentException if Q isn't symmetric positive semidefinite.
+   * @throws IllegalArgumentException if R isn't symmetric positive definite.
+   * @throws IllegalArgumentException if the (A, B) pair isn't stabilizable.
+   * @throws IllegalArgumentException if the (A, C) pair where Q = CᵀC isn't detectable.
+   */
+  public static native void dareABQR(
+      double[] A, double[] B, double[] Q, double[] R, int states, int inputs, double[] S);
+
+  /**
+   * Computes the unique stabilizing solution X to the discrete-time algebraic Riccati equation.
+   *
+   * <p>AᵀXA − X − (AᵀXB + N)(BᵀXB + R)⁻¹(BᵀXA + Nᵀ) + Q = 0
+   *
+   * <p>This overload of the DARE is useful for finding the control law uₖ that minimizes the
+   * following cost function subject to xₖ₊₁ = Axₖ + Buₖ.
+   *
+   * <pre>
+   *     ∞ [xₖ]ᵀ[Q  N][xₖ]
+   * J = Σ [uₖ] [Nᵀ R][uₖ] ΔT
+   *    k=0
+   * </pre>
+   *
+   * <p>This is a more general form of the following. The linear-quadratic regulator is the feedback
+   * control law uₖ that minimizes the following cost function subject to xₖ₊₁ = Axₖ + Buₖ:
+   *
+   * <pre>
+   *     ∞
+   * J = Σ (xₖᵀQxₖ + uₖᵀRuₖ) ΔT
+   *    k=0
+   * </pre>
+   *
+   * <p>This can be refactored as:
+   *
+   * <pre>
+   *     ∞ [xₖ]ᵀ[Q 0][xₖ]
+   * J = Σ [uₖ] [0 R][uₖ] ΔT
+   *    k=0
+   * </pre>
+   *
+   * @param A Array containing elements of A in row-major order.
+   * @param B Array containing elements of B in row-major order.
+   * @param Q Array containing elements of Q in row-major order.
+   * @param R Array containing elements of R in row-major order.
+   * @param N Array containing elements of N in row-major order.
+   * @param states Number of states in A matrix.
+   * @param inputs Number of inputs in B matrix.
+   * @param S Array storage for DARE solution.
+   * @throws IllegalArgumentException if Q − NR⁻¹Nᵀ isn't symmetric positive semidefinite.
+   * @throws IllegalArgumentException if R isn't symmetric positive definite.
+   * @throws IllegalArgumentException if the (A − BR⁻¹Nᵀ, B) pair isn't stabilizable.
+   * @throws IllegalArgumentException if the (A, C) pair where Q = CᵀC isn't detectable.
+   */
+  public static native void dareABQRN(
+      double[] A,
+      double[] B,
+      double[] Q,
+      double[] R,
+      double[] N,
+      int states,
+      int inputs,
+      double[] S);
+
+  /** Sets whether JNI should be loaded in the static block. */
+  public static class Helper {
+    private static AtomicBoolean extractOnStaticLoad = new AtomicBoolean(true);
+
+    /**
+     * Returns true if the JNI should be loaded in the static block.
+     *
+     * @return True if the JNI should be loaded in the static block.
+     */
+    public static boolean getExtractOnStaticLoad() {
+      return extractOnStaticLoad.get();
+    }
+
+    /**
+     * Sets whether the JNI should be loaded in the static block.
+     *
+     * @param load Whether the JNI should be loaded in the static block.
+     */
+    public static void setExtractOnStaticLoad(boolean load) {
+      extractOnStaticLoad.set(load);
+    }
+
+    /** Utility class. */
+    private Helper() {}
+  }
+
+  /** Utility class. */
+  private DAREJNI() {}
+}
diff --git a/wpimath/src/main/java/edu/wpi/first/math/jni/EigenJNI.java b/wpimath/src/main/java/edu/wpi/first/math/jni/EigenJNI.java
new file mode 100644
index 00000000000..ba7f2478d1e
--- /dev/null
+++ b/wpimath/src/main/java/edu/wpi/first/math/jni/EigenJNI.java
@@ -0,0 +1,114 @@
+// 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 edu.wpi.first.math.jni;
+
+import edu.wpi.first.util.RuntimeLoader;
+import java.io.IOException;
+import java.util.concurrent.atomic.AtomicBoolean;
+
+/** Eigen JNI. */
+public final class EigenJNI {
+  static boolean libraryLoaded = false;
+
+  static {
+    if (Helper.getExtractOnStaticLoad()) {
+      try {
+        RuntimeLoader.loadLibrary("wpimathjni");
+      } catch (Exception ex) {
+        ex.printStackTrace();
+        System.exit(1);
+      }
+      libraryLoaded = true;
+    }
+  }
+
+  /**
+   * Force load the library.
+   *
+   * @throws IOException If the library could not be loaded or found.
+   */
+  public static synchronized void forceLoad() throws IOException {
+    if (libraryLoaded) {
+      return;
+    }
+    RuntimeLoader.loadLibrary("wpimathjni");
+    libraryLoaded = true;
+  }
+
+  /**
+   * Computes the matrix exp.
+   *
+   * @param src Array of elements of the matrix to be exponentiated.
+   * @param rows How many rows there are.
+   * @param dst Array where the result will be stored.
+   */
+  public static native void exp(double[] src, int rows, double[] dst);
+
+  /**
+   * Computes the matrix pow.
+   *
+   * @param src Array of elements of the matrix to be raised to a power.
+   * @param rows How many rows there are.
+   * @param exponent The exponent.
+   * @param dst Array where the result will be stored.
+   */
+  public static native void pow(double[] src, int rows, double exponent, double[] dst);
+
+  /**
+   * Performs an inplace rank one update (or downdate) of an upper triangular Cholesky decomposition
+   * matrix.
+   *
+   * @param mat Array of elements of the matrix to be updated.
+   * @param lowerTriangular Whether mat is lower triangular.
+   * @param rows How many rows there are.
+   * @param vec Vector to use for the rank update.
+   * @param sigma Sigma value to use for the rank update.
+   */
+  public static native void rankUpdate(
+      double[] mat, int rows, double[] vec, double sigma, boolean lowerTriangular);
+
+  /**
+   * Solves the least-squares problem Ax=B using a QR decomposition with full pivoting.
+   *
+   * @param A Array of elements of the A matrix.
+   * @param Arows Number of rows of the A matrix.
+   * @param Acols Number of rows of the A matrix.
+   * @param B Array of elements of the B matrix.
+   * @param Brows Number of rows of the B matrix.
+   * @param Bcols Number of rows of the B matrix.
+   * @param dst Array to store solution in. If A is m-n and B is m-p, dst is n-p.
+   */
+  public static native void solveFullPivHouseholderQr(
+      double[] A, int Arows, int Acols, double[] B, int Brows, int Bcols, double[] dst);
+
+  /** Sets whether JNI should be loaded in the static block. */
+  public static class Helper {
+    private static AtomicBoolean extractOnStaticLoad = new AtomicBoolean(true);
+
+    /**
+     * Returns true if the JNI should be loaded in the static block.
+     *
+     * @return True if the JNI should be loaded in the static block.
+     */
+    public static boolean getExtractOnStaticLoad() {
+      return extractOnStaticLoad.get();
+    }
+
+    /**
+     * Sets whether the JNI should be loaded in the static block.
+     *
+     * @param load Whether the JNI should be loaded in the static block.
+     */
+    public static void setExtractOnStaticLoad(boolean load) {
+      extractOnStaticLoad.set(load);
+    }
+
+    /** Utility class. */
+    private Helper() {}
+  }
+
+  /** Utility class. */
+  private EigenJNI() {}
+}
diff --git a/wpimath/src/main/java/edu/wpi/first/math/jni/Ellipse2dJNI.java b/wpimath/src/main/java/edu/wpi/first/math/jni/Ellipse2dJNI.java
new file mode 100644
index 00000000000..e9a5c870677
--- /dev/null
+++ b/wpimath/src/main/java/edu/wpi/first/math/jni/Ellipse2dJNI.java
@@ -0,0 +1,92 @@
+// 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 edu.wpi.first.math.jni;
+
+import edu.wpi.first.util.RuntimeLoader;
+import java.io.IOException;
+import java.util.concurrent.atomic.AtomicBoolean;
+
+/** Ellipse2d JNI. */
+public final class Ellipse2dJNI {
+  static boolean libraryLoaded = false;
+
+  static {
+    if (Helper.getExtractOnStaticLoad()) {
+      try {
+        RuntimeLoader.loadLibrary("wpimathjni");
+      } catch (Exception ex) {
+        ex.printStackTrace();
+        System.exit(1);
+      }
+      libraryLoaded = true;
+    }
+  }
+
+  /**
+   * Force load the library.
+   *
+   * @throws IOException If the library could not be loaded or found.
+   */
+  public static synchronized void forceLoad() throws IOException {
+    if (libraryLoaded) {
+      return;
+    }
+    RuntimeLoader.loadLibrary("wpimathjni");
+    libraryLoaded = true;
+  }
+
+  /**
+   * Returns the nearest point that is contained within the ellipse.
+   *
+   * <p>Constructs an Ellipse2d object and runs its FindNearestPoint() method.
+   *
+   * @param centerX The x coordinate of the center of the ellipse in meters.
+   * @param centerY The y coordinate of the center of the ellipse in meters.
+   * @param centerHeading The ellipse's rotation in radians.
+   * @param xSemiAxis The x semi-axis in meters.
+   * @param ySemiAxis The y semi-axis in meters.
+   * @param pointX The x coordinate of the point that this will find the nearest point to.
+   * @param pointY The y coordinate of the point that this will find the nearest point to.
+   * @param nearestPoint Array to store nearest point into.
+   */
+  public static native void findNearestPoint(
+      double centerX,
+      double centerY,
+      double centerHeading,
+      double xSemiAxis,
+      double ySemiAxis,
+      double pointX,
+      double pointY,
+      double[] nearestPoint);
+
+  /** Sets whether JNI should be loaded in the static block. */
+  public static class Helper {
+    private static AtomicBoolean extractOnStaticLoad = new AtomicBoolean(true);
+
+    /**
+     * Returns true if the JNI should be loaded in the static block.
+     *
+     * @return True if the JNI should be loaded in the static block.
+     */
+    public static boolean getExtractOnStaticLoad() {
+      return extractOnStaticLoad.get();
+    }
+
+    /**
+     * Sets whether the JNI should be loaded in the static block.
+     *
+     * @param load Whether the JNI should be loaded in the static block.
+     */
+    public static void setExtractOnStaticLoad(boolean load) {
+      extractOnStaticLoad.set(load);
+    }
+
+    /** Utility class. */
+    private Helper() {}
+  }
+
+  /** Utility class. */
+  private Ellipse2dJNI() {}
+}
diff --git a/wpimath/src/main/java/edu/wpi/first/math/jni/Pose3dJNI.java b/wpimath/src/main/java/edu/wpi/first/math/jni/Pose3dJNI.java
new file mode 100644
index 00000000000..a642eaa3a98
--- /dev/null
+++ b/wpimath/src/main/java/edu/wpi/first/math/jni/Pose3dJNI.java
@@ -0,0 +1,140 @@
+// 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 edu.wpi.first.math.jni;
+
+import edu.wpi.first.util.RuntimeLoader;
+import java.io.IOException;
+import java.util.concurrent.atomic.AtomicBoolean;
+
+/** Pose3d JNI. */
+public final class Pose3dJNI {
+  static boolean libraryLoaded = false;
+
+  static {
+    if (Helper.getExtractOnStaticLoad()) {
+      try {
+        RuntimeLoader.loadLibrary("wpimathjni");
+      } catch (Exception ex) {
+        ex.printStackTrace();
+        System.exit(1);
+      }
+      libraryLoaded = true;
+    }
+  }
+
+  /**
+   * Force load the library.
+   *
+   * @throws IOException If the library could not be loaded or found.
+   */
+  public static synchronized void forceLoad() throws IOException {
+    if (libraryLoaded) {
+      return;
+    }
+    RuntimeLoader.loadLibrary("wpimathjni");
+    libraryLoaded = true;
+  }
+
+  /**
+   * Obtain a Pose3d from a (constant curvature) velocity.
+   *
+   * <p>The double array returned is of the form [dx, dy, dz, qx, qy, qz].
+   *
+   * @param poseX The pose's translational X component.
+   * @param poseY The pose's translational Y component.
+   * @param poseZ The pose's translational Z component.
+   * @param poseQw The pose quaternion's W component.
+   * @param poseQx The pose quaternion's X component.
+   * @param poseQy The pose quaternion's Y component.
+   * @param poseQz The pose quaternion's Z component.
+   * @param twistDx The twist's dx value.
+   * @param twistDy The twist's dy value.
+   * @param twistDz The twist's dz value.
+   * @param twistRx The twist's rx value.
+   * @param twistRy The twist's ry value.
+   * @param twistRz The twist's rz value.
+   * @return The new pose as a double array.
+   */
+  public static native double[] exp(
+      double poseX,
+      double poseY,
+      double poseZ,
+      double poseQw,
+      double poseQx,
+      double poseQy,
+      double poseQz,
+      double twistDx,
+      double twistDy,
+      double twistDz,
+      double twistRx,
+      double twistRy,
+      double twistRz);
+
+  /**
+   * Returns a Twist3d that maps the starting pose to the end pose.
+   *
+   * <p>The double array returned is of the form [dx, dy, dz, rx, ry, rz].
+   *
+   * @param startX The starting pose's translational X component.
+   * @param startY The starting pose's translational Y component.
+   * @param startZ The starting pose's translational Z component.
+   * @param startQw The starting pose quaternion's W component.
+   * @param startQx The starting pose quaternion's X component.
+   * @param startQy The starting pose quaternion's Y component.
+   * @param startQz The starting pose quaternion's Z component.
+   * @param endX The ending pose's translational X component.
+   * @param endY The ending pose's translational Y component.
+   * @param endZ The ending pose's translational Z component.
+   * @param endQw The ending pose quaternion's W component.
+   * @param endQx The ending pose quaternion's X component.
+   * @param endQy The ending pose quaternion's Y component.
+   * @param endQz The ending pose quaternion's Z component.
+   * @return The twist that maps start to end as a double array.
+   */
+  public static native double[] log(
+      double startX,
+      double startY,
+      double startZ,
+      double startQw,
+      double startQx,
+      double startQy,
+      double startQz,
+      double endX,
+      double endY,
+      double endZ,
+      double endQw,
+      double endQx,
+      double endQy,
+      double endQz);
+
+  /** Sets whether JNI should be loaded in the static block. */
+  public static class Helper {
+    private static AtomicBoolean extractOnStaticLoad = new AtomicBoolean(true);
+
+    /**
+     * Returns true if the JNI should be loaded in the static block.
+     *
+     * @return True if the JNI should be loaded in the static block.
+     */
+    public static boolean getExtractOnStaticLoad() {
+      return extractOnStaticLoad.get();
+    }
+
+    /**
+     * Sets whether the JNI should be loaded in the static block.
+     *
+     * @param load Whether the JNI should be loaded in the static block.
+     */
+    public static void setExtractOnStaticLoad(boolean load) {
+      extractOnStaticLoad.set(load);
+    }
+
+    /** Utility class. */
+    private Helper() {}
+  }
+
+  /** Utility class. */
+  private Pose3dJNI() {}
+}
diff --git a/wpimath/src/main/java/edu/wpi/first/math/jni/StateSpaceUtilJNI.java b/wpimath/src/main/java/edu/wpi/first/math/jni/StateSpaceUtilJNI.java
new file mode 100644
index 00000000000..9ee02b6c4c6
--- /dev/null
+++ b/wpimath/src/main/java/edu/wpi/first/math/jni/StateSpaceUtilJNI.java
@@ -0,0 +1,83 @@
+// 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 edu.wpi.first.math.jni;
+
+import edu.wpi.first.util.RuntimeLoader;
+import java.io.IOException;
+import java.util.concurrent.atomic.AtomicBoolean;
+
+/** StateSpaceUtil JNI. */
+public final class StateSpaceUtilJNI {
+  static boolean libraryLoaded = false;
+
+  static {
+    if (Helper.getExtractOnStaticLoad()) {
+      try {
+        RuntimeLoader.loadLibrary("wpimathjni");
+      } catch (Exception ex) {
+        ex.printStackTrace();
+        System.exit(1);
+      }
+      libraryLoaded = true;
+    }
+  }
+
+  /**
+   * Force load the library.
+   *
+   * @throws IOException If the library could not be loaded or found.
+   */
+  public static synchronized void forceLoad() throws IOException {
+    if (libraryLoaded) {
+      return;
+    }
+    RuntimeLoader.loadLibrary("wpimathjni");
+    libraryLoaded = true;
+  }
+
+  /**
+   * Returns true if (A, B) is a stabilizable pair.
+   *
+   * <p>(A, B) is stabilizable if and only if the uncontrollable eigenvalues of A, if any, have
+   * absolute values less than one, where an eigenvalue is uncontrollable if rank(lambda * I - A, B)
+   * &lt; n where n is the number of states.
+   *
+   * @param states the number of states of the system.
+   * @param inputs the number of inputs to the system.
+   * @param A System matrix.
+   * @param B Input matrix.
+   * @return If the system is stabilizable.
+   */
+  public static native boolean isStabilizable(int states, int inputs, double[] A, double[] B);
+
+  /** Sets whether JNI should be loaded in the static block. */
+  public static class Helper {
+    private static AtomicBoolean extractOnStaticLoad = new AtomicBoolean(true);
+
+    /**
+     * Returns true if the JNI should be loaded in the static block.
+     *
+     * @return True if the JNI should be loaded in the static block.
+     */
+    public static boolean getExtractOnStaticLoad() {
+      return extractOnStaticLoad.get();
+    }
+
+    /**
+     * Sets whether the JNI should be loaded in the static block.
+     *
+     * @param load Whether the JNI should be loaded in the static block.
+     */
+    public static void setExtractOnStaticLoad(boolean load) {
+      extractOnStaticLoad.set(load);
+    }
+
+    /** Utility class. */
+    private Helper() {}
+  }
+
+  /** Utility class. */
+  private StateSpaceUtilJNI() {}
+}
diff --git a/wpimath/src/main/java/edu/wpi/first/math/jni/TrajectoryUtilJNI.java b/wpimath/src/main/java/edu/wpi/first/math/jni/TrajectoryUtilJNI.java
new file mode 100644
index 00000000000..935b0c958b9
--- /dev/null
+++ b/wpimath/src/main/java/edu/wpi/first/math/jni/TrajectoryUtilJNI.java
@@ -0,0 +1,102 @@
+// 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 edu.wpi.first.math.jni;
+
+import edu.wpi.first.util.RuntimeLoader;
+import java.io.IOException;
+import java.util.concurrent.atomic.AtomicBoolean;
+
+/** TrajectoryUtil JNI. */
+public final class TrajectoryUtilJNI {
+  static boolean libraryLoaded = false;
+
+  static {
+    if (Helper.getExtractOnStaticLoad()) {
+      try {
+        RuntimeLoader.loadLibrary("wpimathjni");
+      } catch (Exception ex) {
+        ex.printStackTrace();
+        System.exit(1);
+      }
+      libraryLoaded = true;
+    }
+  }
+
+  /**
+   * Force load the library.
+   *
+   * @throws IOException If the library could not be loaded or found.
+   */
+  public static synchronized void forceLoad() throws IOException {
+    if (libraryLoaded) {
+      return;
+    }
+    RuntimeLoader.loadLibrary("wpimathjni");
+    libraryLoaded = true;
+  }
+
+  /**
+   * Loads a Pathweaver JSON.
+   *
+   * @param path The path to the JSON.
+   * @return A double array with the trajectory states from the JSON.
+   * @throws IOException if the JSON could not be read.
+   */
+  public static native double[] fromPathweaverJson(String path) throws IOException;
+
+  /**
+   * Converts a trajectory into a Pathweaver JSON and saves it.
+   *
+   * @param elements The elements of the trajectory.
+   * @param path The location to save the JSON to.
+   * @throws IOException if the JSON could not be written.
+   */
+  public static native void toPathweaverJson(double[] elements, String path) throws IOException;
+
+  /**
+   * Deserializes a trajectory JSON into a double[] of trajectory elements.
+   *
+   * @param json The JSON containing the serialized trajectory.
+   * @return A double array with the trajectory states.
+   */
+  public static native double[] deserializeTrajectory(String json);
+
+  /**
+   * Serializes the trajectory into a JSON string.
+   *
+   * @param elements The elements of the trajectory.
+   * @return A JSON containing the serialized trajectory.
+   */
+  public static native String serializeTrajectory(double[] elements);
+
+  /** Sets whether JNI should be loaded in the static block. */
+  public static class Helper {
+    private static AtomicBoolean extractOnStaticLoad = new AtomicBoolean(true);
+
+    /**
+     * Returns true if the JNI should be loaded in the static block.
+     *
+     * @return True if the JNI should be loaded in the static block.
+     */
+    public static boolean getExtractOnStaticLoad() {
+      return extractOnStaticLoad.get();
+    }
+
+    /**
+     * Sets whether the JNI should be loaded in the static block.
+     *
+     * @param load Whether the JNI should be loaded in the static block.
+     */
+    public static void setExtractOnStaticLoad(boolean load) {
+      extractOnStaticLoad.set(load);
+    }
+
+    /** Utility class. */
+    private Helper() {}
+  }
+
+  /** Utility class. */
+  private TrajectoryUtilJNI() {}
+}
diff --git a/wpimath/src/main/java/edu/wpi/first/math/trajectory/TrajectoryUtil.java b/wpimath/src/main/java/edu/wpi/first/math/trajectory/TrajectoryUtil.java
index f1b2d6a97b6..19832966df4 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/trajectory/TrajectoryUtil.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/trajectory/TrajectoryUtil.java
@@ -4,9 +4,9 @@
 
 package edu.wpi.first.math.trajectory;
 
-import edu.wpi.first.math.WPIMathJNI;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.jni.TrajectoryUtilJNI;
 import java.io.IOException;
 import java.nio.file.Path;
 import java.util.ArrayList;
@@ -76,7 +76,7 @@ private static double[] getElementsFromTrajectory(Trajectory trajectory) {
    * @throws IOException if reading from the file fails.
    */
   public static Trajectory fromPathweaverJson(Path path) throws IOException {
-    return createTrajectoryFromElements(WPIMathJNI.fromPathweaverJson(path.toString()));
+    return createTrajectoryFromElements(TrajectoryUtilJNI.fromPathweaverJson(path.toString()));
   }
 
   /**
@@ -87,7 +87,7 @@ public static Trajectory fromPathweaverJson(Path path) throws IOException {
    * @throws IOException if writing to the file fails.
    */
   public static void toPathweaverJson(Trajectory trajectory, Path path) throws IOException {
-    WPIMathJNI.toPathweaverJson(getElementsFromTrajectory(trajectory), path.toString());
+    TrajectoryUtilJNI.toPathweaverJson(getElementsFromTrajectory(trajectory), path.toString());
   }
 
   /**
@@ -98,7 +98,7 @@ public static void toPathweaverJson(Trajectory trajectory, Path path) throws IOE
    * @throws TrajectorySerializationException if deserialization of the string fails.
    */
   public static Trajectory deserializeTrajectory(String json) {
-    return createTrajectoryFromElements(WPIMathJNI.deserializeTrajectory(json));
+    return createTrajectoryFromElements(TrajectoryUtilJNI.deserializeTrajectory(json));
   }
 
   /**
@@ -109,7 +109,7 @@ public static Trajectory deserializeTrajectory(String json) {
    * @throws TrajectorySerializationException if serialization of the trajectory fails.
    */
   public static String serializeTrajectory(Trajectory trajectory) {
-    return WPIMathJNI.serializeTrajectory(getElementsFromTrajectory(trajectory));
+    return TrajectoryUtilJNI.serializeTrajectory(getElementsFromTrajectory(trajectory));
   }
 
   /** Exception for trajectory serialization failure. */
diff --git a/wpimath/src/main/native/cpp/jni/WPIMathJNI_ArmFeedforward.cpp b/wpimath/src/main/native/cpp/jni/ArmFeedforwardJNI.cpp
similarity index 86%
rename from wpimath/src/main/native/cpp/jni/WPIMathJNI_ArmFeedforward.cpp
rename to wpimath/src/main/native/cpp/jni/ArmFeedforwardJNI.cpp
index 3428c52688e..ca3a072d644 100644
--- a/wpimath/src/main/native/cpp/jni/WPIMathJNI_ArmFeedforward.cpp
+++ b/wpimath/src/main/native/cpp/jni/ArmFeedforwardJNI.cpp
@@ -6,7 +6,7 @@
 
 #include <wpi/jni_util.h>
 
-#include "edu_wpi_first_math_WPIMathJNI.h"
+#include "edu_wpi_first_math_jni_ArmFeedforwardJNI.h"
 #include "frc/controller/ArmFeedforward.h"
 
 using namespace wpi::java;
@@ -14,12 +14,12 @@ using namespace wpi::java;
 extern "C" {
 
 /*
- * Class:     edu_wpi_first_math_WPIMathJNI
+ * Class:     edu_wpi_first_math_jni_ArmFeedforwardJNI
  * Method:    calculate
  * Signature: (DDDDDDDD)D
  */
 JNIEXPORT jdouble JNICALL
-Java_edu_wpi_first_math_WPIMathJNI_calculate
+Java_edu_wpi_first_math_jni_ArmFeedforwardJNI_calculate
   (JNIEnv* env, jclass, jdouble ks, jdouble kv, jdouble ka, jdouble kg,
    jdouble currentAngle, jdouble currentVelocity, jdouble nextVelocity,
    jdouble dt)
diff --git a/wpimath/src/main/native/cpp/jni/WPIMathJNI_DARE.cpp b/wpimath/src/main/native/cpp/jni/DAREJNI.cpp
similarity index 93%
rename from wpimath/src/main/native/cpp/jni/WPIMathJNI_DARE.cpp
rename to wpimath/src/main/native/cpp/jni/DAREJNI.cpp
index e5c3ac59125..c4b530194bf 100644
--- a/wpimath/src/main/native/cpp/jni/WPIMathJNI_DARE.cpp
+++ b/wpimath/src/main/native/cpp/jni/DAREJNI.cpp
@@ -8,8 +8,8 @@
 
 #include <wpi/jni_util.h>
 
-#include "WPIMathJNI_Exceptions.h"
-#include "edu_wpi_first_math_WPIMathJNI.h"
+#include "Exceptions.h"
+#include "edu_wpi_first_math_jni_DAREJNI.h"
 #include "frc/DARE.h"
 
 using namespace wpi::java;
@@ -17,12 +17,12 @@ using namespace wpi::java;
 extern "C" {
 
 /*
- * Class:     edu_wpi_first_math_WPIMathJNI
+ * Class:     edu_wpi_first_math_jni_DAREJNI
  * Method:    dareDetailABQR
  * Signature: ([D[D[D[DII[D)V
  */
 JNIEXPORT void JNICALL
-Java_edu_wpi_first_math_WPIMathJNI_dareDetailABQR
+Java_edu_wpi_first_math_jni_DAREJNI_dareDetailABQR
   (JNIEnv* env, jclass, jdoubleArray A, jdoubleArray B, jdoubleArray Q,
    jdoubleArray R, jint states, jint inputs, jdoubleArray S)
 {
@@ -54,12 +54,12 @@ Java_edu_wpi_first_math_WPIMathJNI_dareDetailABQR
 }
 
 /*
- * Class:     edu_wpi_first_math_WPIMathJNI
+ * Class:     edu_wpi_first_math_jni_DAREJNI
  * Method:    dareDetailABQRN
  * Signature: ([D[D[D[D[DII[D)V
  */
 JNIEXPORT void JNICALL
-Java_edu_wpi_first_math_WPIMathJNI_dareDetailABQRN
+Java_edu_wpi_first_math_jni_DAREJNI_dareDetailABQRN
   (JNIEnv* env, jclass, jdoubleArray A, jdoubleArray B, jdoubleArray Q,
    jdoubleArray R, jdoubleArray N, jint states, jint inputs, jdoubleArray S)
 {
@@ -95,12 +95,12 @@ Java_edu_wpi_first_math_WPIMathJNI_dareDetailABQRN
 }
 
 /*
- * Class:     edu_wpi_first_math_WPIMathJNI
+ * Class:     edu_wpi_first_math_jni_DAREJNI
  * Method:    dareABQR
  * Signature: ([D[D[D[DII[D)V
  */
 JNIEXPORT void JNICALL
-Java_edu_wpi_first_math_WPIMathJNI_dareABQR
+Java_edu_wpi_first_math_jni_DAREJNI_dareABQR
   (JNIEnv* env, jclass, jdoubleArray A, jdoubleArray B, jdoubleArray Q,
    jdoubleArray R, jint states, jint inputs, jdoubleArray S)
 {
@@ -133,12 +133,12 @@ Java_edu_wpi_first_math_WPIMathJNI_dareABQR
 }
 
 /*
- * Class:     edu_wpi_first_math_WPIMathJNI
+ * Class:     edu_wpi_first_math_jni_DAREJNI
  * Method:    dareABQRN
  * Signature: ([D[D[D[D[DII[D)V
  */
 JNIEXPORT void JNICALL
-Java_edu_wpi_first_math_WPIMathJNI_dareABQRN
+Java_edu_wpi_first_math_jni_DAREJNI_dareABQRN
   (JNIEnv* env, jclass, jdoubleArray A, jdoubleArray B, jdoubleArray Q,
    jdoubleArray R, jdoubleArray N, jint states, jint inputs, jdoubleArray S)
 {
diff --git a/wpimath/src/main/native/cpp/jni/WPIMathJNI_Eigen.cpp b/wpimath/src/main/native/cpp/jni/EigenJNI.cpp
similarity index 87%
rename from wpimath/src/main/native/cpp/jni/WPIMathJNI_Eigen.cpp
rename to wpimath/src/main/native/cpp/jni/EigenJNI.cpp
index 642a2bad69d..e024f994e7a 100644
--- a/wpimath/src/main/native/cpp/jni/WPIMathJNI_Eigen.cpp
+++ b/wpimath/src/main/native/cpp/jni/EigenJNI.cpp
@@ -10,19 +10,19 @@
 #include <unsupported/Eigen/MatrixFunctions>
 #include <wpi/jni_util.h>
 
-#include "edu_wpi_first_math_WPIMathJNI.h"
+#include "edu_wpi_first_math_jni_EigenJNI.h"
 
 using namespace wpi::java;
 
 extern "C" {
 
 /*
- * Class:     edu_wpi_first_math_WPIMathJNI
+ * Class:     edu_wpi_first_math_jni_EigenJNI
  * Method:    exp
  * Signature: ([DI[D)V
  */
 JNIEXPORT void JNICALL
-Java_edu_wpi_first_math_WPIMathJNI_exp
+Java_edu_wpi_first_math_jni_EigenJNI_exp
   (JNIEnv* env, jclass, jdoubleArray src, jint rows, jdoubleArray dst)
 {
   JSpan<const jdouble> arrayBody{env, src};
@@ -37,12 +37,12 @@ Java_edu_wpi_first_math_WPIMathJNI_exp
 }
 
 /*
- * Class:     edu_wpi_first_math_WPIMathJNI
+ * Class:     edu_wpi_first_math_jni_EigenJNI
  * Method:    pow
  * Signature: ([DID[D)V
  */
 JNIEXPORT void JNICALL
-Java_edu_wpi_first_math_WPIMathJNI_pow
+Java_edu_wpi_first_math_jni_EigenJNI_pow
   (JNIEnv* env, jclass, jdoubleArray src, jint rows, jdouble exponent,
    jdoubleArray dst)
 {
@@ -58,12 +58,12 @@ Java_edu_wpi_first_math_WPIMathJNI_pow
 }
 
 /*
- * Class:     edu_wpi_first_math_WPIMathJNI
+ * Class:     edu_wpi_first_math_jni_EigenJNI
  * Method:    rankUpdate
  * Signature: ([DI[DDZ)V
  */
 JNIEXPORT void JNICALL
-Java_edu_wpi_first_math_WPIMathJNI_rankUpdate
+Java_edu_wpi_first_math_jni_EigenJNI_rankUpdate
   (JNIEnv* env, jclass, jdoubleArray mat, jint rows, jdoubleArray vec,
    jdouble sigma, jboolean lowerTriangular)
 {
@@ -84,12 +84,12 @@ Java_edu_wpi_first_math_WPIMathJNI_rankUpdate
 }
 
 /*
- * Class:     edu_wpi_first_math_WPIMathJNI
+ * Class:     edu_wpi_first_math_jni_EigenJNI
  * Method:    solveFullPivHouseholderQr
  * Signature: ([DII[DII[D)V
  */
 JNIEXPORT void JNICALL
-Java_edu_wpi_first_math_WPIMathJNI_solveFullPivHouseholderQr
+Java_edu_wpi_first_math_jni_EigenJNI_solveFullPivHouseholderQr
   (JNIEnv* env, jclass, jdoubleArray A, jint Arows, jint Acols, jdoubleArray B,
    jint Brows, jint Bcols, jdoubleArray dst)
 {
diff --git a/wpimath/src/main/native/cpp/jni/WPIMathJNI_Ellipse2d.cpp b/wpimath/src/main/native/cpp/jni/Ellipse2dJNI.cpp
similarity index 84%
rename from wpimath/src/main/native/cpp/jni/WPIMathJNI_Ellipse2d.cpp
rename to wpimath/src/main/native/cpp/jni/Ellipse2dJNI.cpp
index ae6f88a21ee..cc53d693154 100644
--- a/wpimath/src/main/native/cpp/jni/WPIMathJNI_Ellipse2d.cpp
+++ b/wpimath/src/main/native/cpp/jni/Ellipse2dJNI.cpp
@@ -7,7 +7,7 @@
 #include <wpi/array.h>
 #include <wpi/jni_util.h>
 
-#include "edu_wpi_first_math_WPIMathJNI.h"
+#include "edu_wpi_first_math_jni_Ellipse2dJNI.h"
 #include "frc/geometry/Ellipse2d.h"
 
 using namespace wpi::java;
@@ -15,12 +15,12 @@ using namespace wpi::java;
 extern "C" {
 
 /*
- * Class:     edu_wpi_first_math_WPIMathJNI
- * Method:    ellipse2dFindNearestPoint
+ * Class:     edu_wpi_first_math_jni_Ellipse2dJNI
+ * Method:    findNearestPoint
  * Signature: (DDDDDDD[D)V
  */
 JNIEXPORT void JNICALL
-Java_edu_wpi_first_math_WPIMathJNI_ellipse2dFindNearestPoint
+Java_edu_wpi_first_math_jni_Ellipse2dJNI_findNearestPoint
   (JNIEnv* env, jclass, jdouble centerX, jdouble centerY, jdouble centerHeading,
    jdouble xSemiAxis, jdouble ySemiAxis, jdouble pointX, jdouble pointY,
    jdoubleArray nearestPoint)
diff --git a/wpimath/src/main/native/cpp/jni/WPIMathJNI_Exceptions.cpp b/wpimath/src/main/native/cpp/jni/Exceptions.cpp
similarity index 97%
rename from wpimath/src/main/native/cpp/jni/WPIMathJNI_Exceptions.cpp
rename to wpimath/src/main/native/cpp/jni/Exceptions.cpp
index 7f5a03f6fa0..6e217bfafc0 100644
--- a/wpimath/src/main/native/cpp/jni/WPIMathJNI_Exceptions.cpp
+++ b/wpimath/src/main/native/cpp/jni/Exceptions.cpp
@@ -2,7 +2,7 @@
 // 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.
 
-#include "WPIMathJNI_Exceptions.h"
+#include "Exceptions.h"
 
 #include <jni.h>
 
diff --git a/wpimath/src/main/native/cpp/jni/WPIMathJNI_Exceptions.h b/wpimath/src/main/native/cpp/jni/Exceptions.h
similarity index 100%
rename from wpimath/src/main/native/cpp/jni/WPIMathJNI_Exceptions.h
rename to wpimath/src/main/native/cpp/jni/Exceptions.h
diff --git a/wpimath/src/main/native/cpp/jni/WPIMathJNI_Pose3d.cpp b/wpimath/src/main/native/cpp/jni/Pose3dJNI.cpp
similarity index 89%
rename from wpimath/src/main/native/cpp/jni/WPIMathJNI_Pose3d.cpp
rename to wpimath/src/main/native/cpp/jni/Pose3dJNI.cpp
index f985231e547..e41e0eb7030 100644
--- a/wpimath/src/main/native/cpp/jni/WPIMathJNI_Pose3d.cpp
+++ b/wpimath/src/main/native/cpp/jni/Pose3dJNI.cpp
@@ -6,7 +6,7 @@
 
 #include <wpi/jni_util.h>
 
-#include "edu_wpi_first_math_WPIMathJNI.h"
+#include "edu_wpi_first_math_jni_Pose3dJNI.h"
 #include "frc/geometry/Pose3d.h"
 
 using namespace wpi::java;
@@ -14,12 +14,12 @@ using namespace wpi::java;
 extern "C" {
 
 /*
- * Class:     edu_wpi_first_math_WPIMathJNI
- * Method:    expPose3d
+ * Class:     edu_wpi_first_math_jni_Pose3dJNI
+ * Method:    exp
  * Signature: (DDDDDDDDDDDDD)[D
  */
 JNIEXPORT jdoubleArray JNICALL
-Java_edu_wpi_first_math_WPIMathJNI_expPose3d
+Java_edu_wpi_first_math_jni_Pose3dJNI_exp
   (JNIEnv* env, jclass, jdouble poseX, jdouble poseY, jdouble poseZ,
    jdouble poseQw, jdouble poseQx, jdouble poseQy, jdouble poseQz,
    jdouble twistDx, jdouble twistDy, jdouble twistDz, jdouble twistRx,
@@ -42,12 +42,12 @@ Java_edu_wpi_first_math_WPIMathJNI_expPose3d
 }
 
 /*
- * Class:     edu_wpi_first_math_WPIMathJNI
- * Method:    logPose3d
+ * Class:     edu_wpi_first_math_jni_Pose3dJNI
+ * Method:    log
  * Signature: (DDDDDDDDDDDDDD)[D
  */
 JNIEXPORT jdoubleArray JNICALL
-Java_edu_wpi_first_math_WPIMathJNI_logPose3d
+Java_edu_wpi_first_math_jni_Pose3dJNI_log
   (JNIEnv* env, jclass, jdouble startX, jdouble startY, jdouble startZ,
    jdouble startQw, jdouble startQx, jdouble startQy, jdouble startQz,
    jdouble endX, jdouble endY, jdouble endZ, jdouble endQw, jdouble endQx,
diff --git a/wpimath/src/main/native/cpp/jni/WPIMathJNI_StateSpaceUtil.cpp b/wpimath/src/main/native/cpp/jni/StateSpaceUtilJNI.cpp
similarity index 86%
rename from wpimath/src/main/native/cpp/jni/WPIMathJNI_StateSpaceUtil.cpp
rename to wpimath/src/main/native/cpp/jni/StateSpaceUtilJNI.cpp
index dc3dfddf33d..e13d659bf2d 100644
--- a/wpimath/src/main/native/cpp/jni/WPIMathJNI_StateSpaceUtil.cpp
+++ b/wpimath/src/main/native/cpp/jni/StateSpaceUtilJNI.cpp
@@ -7,7 +7,7 @@
 #include <Eigen/Core>
 #include <wpi/jni_util.h>
 
-#include "edu_wpi_first_math_WPIMathJNI.h"
+#include "edu_wpi_first_math_jni_StateSpaceUtilJNI.h"
 #include "frc/StateSpaceUtil.h"
 
 using namespace wpi::java;
@@ -15,12 +15,12 @@ using namespace wpi::java;
 extern "C" {
 
 /*
- * Class:     edu_wpi_first_math_WPIMathJNI
+ * Class:     edu_wpi_first_math_jni_StateSpaceUtilJNI
  * Method:    isStabilizable
  * Signature: (II[D[D)Z
  */
 JNIEXPORT jboolean JNICALL
-Java_edu_wpi_first_math_WPIMathJNI_isStabilizable
+Java_edu_wpi_first_math_jni_StateSpaceUtilJNI_isStabilizable
   (JNIEnv* env, jclass, jint states, jint inputs, jdoubleArray aSrc,
    jdoubleArray bSrc)
 {
diff --git a/wpimath/src/main/native/cpp/jni/WPIMathJNI_Trajectory.cpp b/wpimath/src/main/native/cpp/jni/TrajectoryUtilJNI.cpp
similarity index 86%
rename from wpimath/src/main/native/cpp/jni/WPIMathJNI_Trajectory.cpp
rename to wpimath/src/main/native/cpp/jni/TrajectoryUtilJNI.cpp
index 3359da984ca..1066e037c8c 100644
--- a/wpimath/src/main/native/cpp/jni/WPIMathJNI_Trajectory.cpp
+++ b/wpimath/src/main/native/cpp/jni/TrajectoryUtilJNI.cpp
@@ -8,8 +8,8 @@
 
 #include <wpi/jni_util.h>
 
-#include "WPIMathJNI_Exceptions.h"
-#include "edu_wpi_first_math_WPIMathJNI.h"
+#include "Exceptions.h"
+#include "edu_wpi_first_math_jni_TrajectoryUtilJNI.h"
 #include "frc/trajectory/TrajectoryUtil.h"
 
 using namespace wpi::java;
@@ -57,12 +57,12 @@ frc::Trajectory CreateTrajectoryFromElements(std::span<const double> elements) {
 extern "C" {
 
 /*
- * Class:     edu_wpi_first_math_WPIMathJNI
+ * Class:     edu_wpi_first_math_jni_TrajectoryUtilJNI
  * Method:    fromPathweaverJson
  * Signature: (Ljava/lang/String;)[D
  */
 JNIEXPORT jdoubleArray JNICALL
-Java_edu_wpi_first_math_WPIMathJNI_fromPathweaverJson
+Java_edu_wpi_first_math_jni_TrajectoryUtilJNI_fromPathweaverJson
   (JNIEnv* env, jclass, jstring path)
 {
   try {
@@ -77,12 +77,12 @@ Java_edu_wpi_first_math_WPIMathJNI_fromPathweaverJson
 }
 
 /*
- * Class:     edu_wpi_first_math_WPIMathJNI
+ * Class:     edu_wpi_first_math_jni_TrajectoryUtilJNI
  * Method:    toPathweaverJson
  * Signature: ([DLjava/lang/String;)V
  */
 JNIEXPORT void JNICALL
-Java_edu_wpi_first_math_WPIMathJNI_toPathweaverJson
+Java_edu_wpi_first_math_jni_TrajectoryUtilJNI_toPathweaverJson
   (JNIEnv* env, jclass, jdoubleArray elements, jstring path)
 {
   try {
@@ -96,12 +96,12 @@ Java_edu_wpi_first_math_WPIMathJNI_toPathweaverJson
 }
 
 /*
- * Class:     edu_wpi_first_math_WPIMathJNI
+ * Class:     edu_wpi_first_math_jni_TrajectoryUtilJNI
  * Method:    deserializeTrajectory
  * Signature: (Ljava/lang/String;)[D
  */
 JNIEXPORT jdoubleArray JNICALL
-Java_edu_wpi_first_math_WPIMathJNI_deserializeTrajectory
+Java_edu_wpi_first_math_jni_TrajectoryUtilJNI_deserializeTrajectory
   (JNIEnv* env, jclass, jstring json)
 {
   try {
@@ -116,12 +116,12 @@ Java_edu_wpi_first_math_WPIMathJNI_deserializeTrajectory
 }
 
 /*
- * Class:     edu_wpi_first_math_WPIMathJNI
+ * Class:     edu_wpi_first_math_jni_TrajectoryUtilJNI
  * Method:    serializeTrajectory
  * Signature: ([D)Ljava/lang/String;
  */
 JNIEXPORT jstring JNICALL
-Java_edu_wpi_first_math_WPIMathJNI_serializeTrajectory
+Java_edu_wpi_first_math_jni_TrajectoryUtilJNI_serializeTrajectory
   (JNIEnv* env, jclass, jdoubleArray elements)
 {
   try {
diff --git a/wpimath/src/test/java/edu/wpi/first/math/jni/ArmFeedforwardJNITest.java b/wpimath/src/test/java/edu/wpi/first/math/jni/ArmFeedforwardJNITest.java
new file mode 100644
index 00000000000..7fadaa2c40f
--- /dev/null
+++ b/wpimath/src/test/java/edu/wpi/first/math/jni/ArmFeedforwardJNITest.java
@@ -0,0 +1,16 @@
+// 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 edu.wpi.first.math.jni;
+
+import static org.junit.jupiter.api.Assertions.assertDoesNotThrow;
+
+import org.junit.jupiter.api.Test;
+
+public class ArmFeedforwardJNITest {
+  @Test
+  public void testLink() {
+    assertDoesNotThrow(ArmFeedforwardJNI::forceLoad);
+  }
+}
diff --git a/wpimath/src/test/java/edu/wpi/first/math/WPIMathJNITest.java b/wpimath/src/test/java/edu/wpi/first/math/jni/DAREJNITest.java
similarity index 76%
rename from wpimath/src/test/java/edu/wpi/first/math/WPIMathJNITest.java
rename to wpimath/src/test/java/edu/wpi/first/math/jni/DAREJNITest.java
index 6a1bf2edaff..88d0486554e 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/WPIMathJNITest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/jni/DAREJNITest.java
@@ -2,15 +2,15 @@
 // 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 edu.wpi.first.math;
+package edu.wpi.first.math.jni;
 
 import static org.junit.jupiter.api.Assertions.assertDoesNotThrow;
 
 import org.junit.jupiter.api.Test;
 
-public class WPIMathJNITest {
+public class DAREJNITest {
   @Test
   public void testLink() {
-    assertDoesNotThrow(WPIMathJNI::forceLoad);
+    assertDoesNotThrow(DAREJNI::forceLoad);
   }
 }
diff --git a/wpimath/src/test/java/edu/wpi/first/math/jni/EigenJNITest.java b/wpimath/src/test/java/edu/wpi/first/math/jni/EigenJNITest.java
new file mode 100644
index 00000000000..ad98fefbf55
--- /dev/null
+++ b/wpimath/src/test/java/edu/wpi/first/math/jni/EigenJNITest.java
@@ -0,0 +1,16 @@
+// 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 edu.wpi.first.math.jni;
+
+import static org.junit.jupiter.api.Assertions.assertDoesNotThrow;
+
+import org.junit.jupiter.api.Test;
+
+public class EigenJNITest {
+  @Test
+  public void testLink() {
+    assertDoesNotThrow(EigenJNI::forceLoad);
+  }
+}
diff --git a/wpimath/src/test/java/edu/wpi/first/math/jni/Ellipse2dJNITest.java b/wpimath/src/test/java/edu/wpi/first/math/jni/Ellipse2dJNITest.java
new file mode 100644
index 00000000000..dbdb97f7b84
--- /dev/null
+++ b/wpimath/src/test/java/edu/wpi/first/math/jni/Ellipse2dJNITest.java
@@ -0,0 +1,16 @@
+// 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 edu.wpi.first.math.jni;
+
+import static org.junit.jupiter.api.Assertions.assertDoesNotThrow;
+
+import org.junit.jupiter.api.Test;
+
+public class Ellipse2dJNITest {
+  @Test
+  public void testLink() {
+    assertDoesNotThrow(Ellipse2dJNI::forceLoad);
+  }
+}
diff --git a/wpimath/src/test/java/edu/wpi/first/math/jni/Pose3dJNITest.java b/wpimath/src/test/java/edu/wpi/first/math/jni/Pose3dJNITest.java
new file mode 100644
index 00000000000..d7f90a98e27
--- /dev/null
+++ b/wpimath/src/test/java/edu/wpi/first/math/jni/Pose3dJNITest.java
@@ -0,0 +1,16 @@
+// 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 edu.wpi.first.math.jni;
+
+import static org.junit.jupiter.api.Assertions.assertDoesNotThrow;
+
+import org.junit.jupiter.api.Test;
+
+public class Pose3dJNITest {
+  @Test
+  public void testLink() {
+    assertDoesNotThrow(Pose3dJNI::forceLoad);
+  }
+}
diff --git a/wpimath/src/test/java/edu/wpi/first/math/jni/StateSpaceUtilJNITest.java b/wpimath/src/test/java/edu/wpi/first/math/jni/StateSpaceUtilJNITest.java
new file mode 100644
index 00000000000..a7d9cad06d6
--- /dev/null
+++ b/wpimath/src/test/java/edu/wpi/first/math/jni/StateSpaceUtilJNITest.java
@@ -0,0 +1,16 @@
+// 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 edu.wpi.first.math.jni;
+
+import static org.junit.jupiter.api.Assertions.assertDoesNotThrow;
+
+import org.junit.jupiter.api.Test;
+
+public class StateSpaceUtilJNITest {
+  @Test
+  public void testLink() {
+    assertDoesNotThrow(StateSpaceUtilJNI::forceLoad);
+  }
+}
diff --git a/wpimath/src/test/java/edu/wpi/first/math/jni/TrajectoryUtilJNITest.java b/wpimath/src/test/java/edu/wpi/first/math/jni/TrajectoryUtilJNITest.java
new file mode 100644
index 00000000000..9513995f137
--- /dev/null
+++ b/wpimath/src/test/java/edu/wpi/first/math/jni/TrajectoryUtilJNITest.java
@@ -0,0 +1,16 @@
+// 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 edu.wpi.first.math.jni;
+
+import static org.junit.jupiter.api.Assertions.assertDoesNotThrow;
+
+import org.junit.jupiter.api.Test;
+
+public class TrajectoryUtilJNITest {
+  @Test
+  public void testLink() {
+    assertDoesNotThrow(TrajectoryUtilJNI::forceLoad);
+  }
+}