diff --git a/ntcore/src/main/native/cpp/jni/NetworkTablesJNI.cpp b/ntcore/src/main/native/cpp/jni/NetworkTablesJNI.cpp
index d4679a3c21..54d5a855a8 100644
--- a/ntcore/src/main/native/cpp/jni/NetworkTablesJNI.cpp
+++ b/ntcore/src/main/native/cpp/jni/NetworkTablesJNI.cpp
@@ -31,7 +31,6 @@ void JNI_UnloadTypes(JNIEnv* env);
 //
 
 // Used for callback.
-static JavaVM* jvm = nullptr;
 static JClass booleanCls;
 static JClass connectionInfoCls;
 static JClass doubleCls;
@@ -72,8 +71,6 @@ static const JExceptionInit exceptions[] = {
 extern "C" {
 
 JNIEXPORT jint JNICALL JNI_OnLoad(JavaVM* vm, void* reserved) {
-  jvm = vm;
-
   JNIEnv* env;
   if (vm->GetEnv(reinterpret_cast<void**>(&env), JNI_VERSION_1_6) != JNI_OK) {
     return JNI_ERR;
@@ -114,7 +111,6 @@ JNIEXPORT void JNICALL JNI_OnUnload(JavaVM* vm, void* reserved) {
     c.cls->free(env);
   }
   nt::JNI_UnloadTypes(env);
-  jvm = nullptr;
 }
 
 }  // extern "C"
diff --git a/wpimath/CMakeLists.txt b/wpimath/CMakeLists.txt
index 01fd5d0f8e..7c7458d248 100644
--- a/wpimath/CMakeLists.txt
+++ b/wpimath/CMakeLists.txt
@@ -4,7 +4,12 @@ include(SubDirList)
 include(CompileWarnings)
 include(AddTest)
 
-file(GLOB wpimath_jni_src src/main/native/cpp/jni/WPIMathJNI.cpp)
+file(GLOB wpimath_jni_src src/main/native/cpp/jni/WPIMathJNI_DARE.cpp
+                          src/main/native/cpp/jni/WPIMathJNI_Eigen.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)
 
 # Java bindings
 if (WITH_JAVA)
diff --git a/wpimath/src/main/java/edu/wpi/first/math/WPIMathJNI.java b/wpimath/src/main/java/edu/wpi/first/math/WPIMathJNI.java
index 71046c8c58..ef47a3b3c2 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/WPIMathJNI.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/WPIMathJNI.java
@@ -43,6 +43,8 @@ public final class WPIMathJNI {
     libraryLoaded = true;
   }
 
+  // DARE wrappers
+
   /**
    * Computes the unique stabilizing solution X to the discrete-time algebraic Riccati equation.
    *
@@ -208,6 +210,8 @@ public final class WPIMathJNI {
       int inputs,
       double[] S);
 
+  // Eigen wrappers
+
   /**
    * Computes the matrix exp.
    *
@@ -227,6 +231,35 @@ public final class WPIMathJNI {
    */
   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);
+
+  // Pose3d wrappers
+
   /**
    * Obtain a Pose3d from a (constant curvature) velocity.
    *
@@ -299,6 +332,8 @@ public final class WPIMathJNI {
       double endQy,
       double endQz);
 
+  // StateSpaceUtil wrappers
+
   /**
    * Returns true if (A, B) is a stabilizable pair.
    *
@@ -314,6 +349,8 @@ public final class WPIMathJNI {
    */
   public static native boolean isStabilizable(int states, int inputs, double[] A, double[] B);
 
+  // Trajectory wrappers
+
   /**
    * Loads a Pathweaver JSON.
    *
@@ -348,19 +385,6 @@ public final class WPIMathJNI {
    */
   public static native String serializeTrajectory(double[] elements);
 
-  /**
-   * 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);
-
   public static class Helper {
     private static AtomicBoolean extractOnStaticLoad = new AtomicBoolean(true);
 
@@ -372,18 +396,4 @@ public final class WPIMathJNI {
       extractOnStaticLoad.set(load);
     }
   }
-
-  /**
-   * 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);
 }
diff --git a/wpimath/src/main/native/cpp/jni/WPIMathJNI.cpp b/wpimath/src/main/native/cpp/jni/WPIMathJNI.cpp
deleted file mode 100644
index 56461cec76..0000000000
--- a/wpimath/src/main/native/cpp/jni/WPIMathJNI.cpp
+++ /dev/null
@@ -1,498 +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.
-
-#include <jni.h>
-
-#include <exception>
-#include <stdexcept>
-
-#include <Eigen/Cholesky>
-#include <Eigen/Core>
-#include <Eigen/QR>
-#include <unsupported/Eigen/MatrixFunctions>
-#include <wpi/jni_util.h>
-
-#include "edu_wpi_first_math_WPIMathJNI.h"
-#include "frc/DARE.h"
-#include "frc/geometry/Pose3d.h"
-#include "frc/trajectory/TrajectoryUtil.h"
-
-using namespace wpi::java;
-
-std::vector<double> GetElementsFromTrajectory(
-    const frc::Trajectory& trajectory) {
-  std::vector<double> elements;
-  elements.reserve(trajectory.States().size() * 7);
-
-  for (auto&& state : trajectory.States()) {
-    elements.push_back(state.t.value());
-    elements.push_back(state.velocity.value());
-    elements.push_back(state.acceleration.value());
-    elements.push_back(state.pose.X().value());
-    elements.push_back(state.pose.Y().value());
-    elements.push_back(state.pose.Rotation().Radians().value());
-    elements.push_back(state.curvature.value());
-  }
-
-  return elements;
-}
-
-frc::Trajectory CreateTrajectoryFromElements(std::span<const double> elements) {
-  // Make sure that the elements have the correct length.
-  assert(elements.size() % 7 == 0);
-
-  // Create a vector of states from the elements.
-  std::vector<frc::Trajectory::State> states;
-  states.reserve(elements.size() / 7);
-
-  for (size_t i = 0; i < elements.size(); i += 7) {
-    states.emplace_back(frc::Trajectory::State{
-        units::second_t{elements[i]},
-        units::meters_per_second_t{elements[i + 1]},
-        units::meters_per_second_squared_t{elements[i + 2]},
-        frc::Pose2d{units::meter_t{elements[i + 3]},
-                    units::meter_t{elements[i + 4]},
-                    units::radian_t{elements[i + 5]}},
-        units::curvature_t{elements[i + 6]}});
-  }
-
-  return frc::Trajectory(states);
-}
-
-extern "C" {
-
-/*
- * Class:     edu_wpi_first_math_WPIMathJNI
- * Method:    dareDetailABQR
- * Signature: ([D[D[D[DII[D)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_math_WPIMathJNI_dareDetailABQR
-  (JNIEnv* env, jclass, jdoubleArray A, jdoubleArray B, jdoubleArray Q,
-   jdoubleArray R, jint states, jint inputs, jdoubleArray S)
-{
-  JSpan<const jdouble> nativeA{env, A};
-  JSpan<const jdouble> nativeB{env, B};
-  JSpan<const jdouble> nativeQ{env, Q};
-  JSpan<const jdouble> nativeR{env, R};
-
-  Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
-                                 Eigen::RowMajor>>
-      Amat{nativeA.data(), states, states};
-  Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
-                                 Eigen::RowMajor>>
-      Bmat{nativeB.data(), states, inputs};
-  Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
-                                 Eigen::RowMajor>>
-      Qmat{nativeQ.data(), states, states};
-  Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
-                                 Eigen::RowMajor>>
-      Rmat{nativeR.data(), inputs, inputs};
-
-  Eigen::MatrixXd RmatCopy{Rmat};
-  auto R_llt = RmatCopy.llt();
-
-  Eigen::MatrixXd result = frc::detail::DARE<Eigen::Dynamic, Eigen::Dynamic>(
-      Amat, Bmat, Qmat, R_llt);
-
-  env->SetDoubleArrayRegion(S, 0, states * states, result.data());
-}
-
-/*
- * Class:     edu_wpi_first_math_WPIMathJNI
- * Method:    dareDetailABQRN
- * Signature: ([D[D[D[D[DII[D)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_math_WPIMathJNI_dareDetailABQRN
-  (JNIEnv* env, jclass, jdoubleArray A, jdoubleArray B, jdoubleArray Q,
-   jdoubleArray R, jdoubleArray N, jint states, jint inputs, jdoubleArray S)
-{
-  JSpan<const jdouble> nativeA{env, A};
-  JSpan<const jdouble> nativeB{env, B};
-  JSpan<const jdouble> nativeQ{env, Q};
-  JSpan<const jdouble> nativeR{env, R};
-  JSpan<const jdouble> nativeN{env, N};
-
-  Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
-                                 Eigen::RowMajor>>
-      Amat{nativeA.data(), states, states};
-  Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
-                                 Eigen::RowMajor>>
-      Bmat{nativeB.data(), states, inputs};
-  Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
-                                 Eigen::RowMajor>>
-      Qmat{nativeQ.data(), states, states};
-  Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
-                                 Eigen::RowMajor>>
-      Rmat{nativeR.data(), inputs, inputs};
-  Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
-                                 Eigen::RowMajor>>
-      Nmat{nativeN.data(), states, inputs};
-
-  Eigen::MatrixXd Rcopy{Rmat};
-  auto R_llt = Rcopy.llt();
-
-  Eigen::MatrixXd result = frc::detail::DARE<Eigen::Dynamic, Eigen::Dynamic>(
-      Amat, Bmat, Qmat, R_llt, Nmat);
-
-  env->SetDoubleArrayRegion(S, 0, states * states, result.data());
-}
-
-/*
- * Class:     edu_wpi_first_math_WPIMathJNI
- * Method:    dareABQR
- * Signature: ([D[D[D[DII[D)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_math_WPIMathJNI_dareABQR
-  (JNIEnv* env, jclass, jdoubleArray A, jdoubleArray B, jdoubleArray Q,
-   jdoubleArray R, jint states, jint inputs, jdoubleArray S)
-{
-  JSpan<const jdouble> nativeA{env, A};
-  JSpan<const jdouble> nativeB{env, B};
-  JSpan<const jdouble> nativeQ{env, Q};
-  JSpan<const jdouble> nativeR{env, R};
-
-  Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
-                                 Eigen::RowMajor>>
-      Amat{nativeA.data(), states, states};
-  Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
-                                 Eigen::RowMajor>>
-      Bmat{nativeB.data(), states, inputs};
-  Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
-                                 Eigen::RowMajor>>
-      Qmat{nativeQ.data(), states, states};
-  Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
-                                 Eigen::RowMajor>>
-      Rmat{nativeR.data(), inputs, inputs};
-
-  try {
-    Eigen::MatrixXd result =
-        frc::DARE<Eigen::Dynamic, Eigen::Dynamic>(Amat, Bmat, Qmat, Rmat);
-
-    env->SetDoubleArrayRegion(S, 0, states * states, result.data());
-  } catch (const std::invalid_argument& e) {
-    jclass cls = env->FindClass("java/lang/IllegalArgumentException");
-    if (cls) {
-      env->ThrowNew(cls, e.what());
-    }
-  }
-}
-
-/*
- * Class:     edu_wpi_first_math_WPIMathJNI
- * Method:    dareABQRN
- * Signature: ([D[D[D[D[DII[D)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_math_WPIMathJNI_dareABQRN
-  (JNIEnv* env, jclass, jdoubleArray A, jdoubleArray B, jdoubleArray Q,
-   jdoubleArray R, jdoubleArray N, jint states, jint inputs, jdoubleArray S)
-{
-  JSpan<const jdouble> nativeA{env, A};
-  JSpan<const jdouble> nativeB{env, B};
-  JSpan<const jdouble> nativeQ{env, Q};
-  JSpan<const jdouble> nativeR{env, R};
-  JSpan<const jdouble> nativeN{env, N};
-
-  Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
-                                 Eigen::RowMajor>>
-      Amat{nativeA.data(), states, states};
-  Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
-                                 Eigen::RowMajor>>
-      Bmat{nativeB.data(), states, inputs};
-  Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
-                                 Eigen::RowMajor>>
-      Qmat{nativeQ.data(), states, states};
-  Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
-                                 Eigen::RowMajor>>
-      Rmat{nativeR.data(), inputs, inputs};
-  Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
-                                 Eigen::RowMajor>>
-      Nmat{nativeN.data(), states, inputs};
-
-  try {
-    Eigen::MatrixXd result =
-        frc::DARE<Eigen::Dynamic, Eigen::Dynamic>(Amat, Bmat, Qmat, Rmat, Nmat);
-
-    env->SetDoubleArrayRegion(S, 0, states * states, result.data());
-  } catch (const std::invalid_argument& e) {
-    jclass cls = env->FindClass("java/lang/IllegalArgumentException");
-    if (cls) {
-      env->ThrowNew(cls, e.what());
-    }
-  }
-}
-
-/*
- * Class:     edu_wpi_first_math_WPIMathJNI
- * Method:    exp
- * Signature: ([DI[D)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_math_WPIMathJNI_exp
-  (JNIEnv* env, jclass, jdoubleArray src, jint rows, jdoubleArray dst)
-{
-  JSpan<const jdouble> arrayBody{env, src};
-
-  Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
-                                 Eigen::RowMajor>>
-      Amat{arrayBody.data(), rows, rows};
-  Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> Aexp =
-      Amat.exp();
-
-  env->SetDoubleArrayRegion(dst, 0, rows * rows, Aexp.data());
-}
-
-/*
- * Class:     edu_wpi_first_math_WPIMathJNI
- * Method:    pow
- * Signature: ([DID[D)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_math_WPIMathJNI_pow
-  (JNIEnv* env, jclass, jdoubleArray src, jint rows, jdouble exponent,
-   jdoubleArray dst)
-{
-  JSpan<const jdouble> arrayBody{env, src};
-
-  Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
-                                 Eigen::RowMajor>>
-      Amat{arrayBody.data(), rows, rows};  // NOLINT
-  Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> Apow =
-      Amat.pow(exponent);
-
-  env->SetDoubleArrayRegion(dst, 0, rows * rows, Apow.data());
-}
-
-/*
- * Class:     edu_wpi_first_math_WPIMathJNI
- * Method:    expPose3d
- * Signature: (DDDDDDDDDDDDD)[D
- */
-JNIEXPORT jdoubleArray JNICALL
-Java_edu_wpi_first_math_WPIMathJNI_expPose3d
-  (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,
-   jdouble twistRy, jdouble twistRz)
-{
-  frc::Pose3d pose{
-      units::meter_t{poseX}, units::meter_t{poseY}, units::meter_t{poseZ},
-      frc::Rotation3d{frc::Quaternion{poseQw, poseQx, poseQy, poseQz}}};
-  frc::Twist3d twist{units::meter_t{twistDx},  units::meter_t{twistDy},
-                     units::meter_t{twistDz},  units::radian_t{twistRx},
-                     units::radian_t{twistRy}, units::radian_t{twistRz}};
-
-  frc::Pose3d result = pose.Exp(twist);
-
-  const auto& resultQuaternion = result.Rotation().GetQuaternion();
-  return MakeJDoubleArray(
-      env, {{result.X().value(), result.Y().value(), result.Z().value(),
-             resultQuaternion.W(), resultQuaternion.X(), resultQuaternion.Y(),
-             resultQuaternion.Z()}});
-}
-
-/*
- * Class:     edu_wpi_first_math_WPIMathJNI
- * Method:    logPose3d
- * Signature: (DDDDDDDDDDDDDD)[D
- */
-JNIEXPORT jdoubleArray JNICALL
-Java_edu_wpi_first_math_WPIMathJNI_logPose3d
-  (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,
-   jdouble endQy, jdouble endQz)
-{
-  frc::Pose3d startPose{
-      units::meter_t{startX}, units::meter_t{startY}, units::meter_t{startZ},
-      frc::Rotation3d{frc::Quaternion{startQw, startQx, startQy, startQz}}};
-  frc::Pose3d endPose{
-      units::meter_t{endX}, units::meter_t{endY}, units::meter_t{endZ},
-      frc::Rotation3d{frc::Quaternion{endQw, endQx, endQy, endQz}}};
-
-  frc::Twist3d result = startPose.Log(endPose);
-
-  return MakeJDoubleArray(
-      env, {{result.dx.value(), result.dy.value(), result.dz.value(),
-             result.rx.value(), result.ry.value(), result.rz.value()}});
-}
-
-/*
- * Class:     edu_wpi_first_math_WPIMathJNI
- * Method:    isStabilizable
- * Signature: (II[D[D)Z
- */
-JNIEXPORT jboolean JNICALL
-Java_edu_wpi_first_math_WPIMathJNI_isStabilizable
-  (JNIEnv* env, jclass, jint states, jint inputs, jdoubleArray aSrc,
-   jdoubleArray bSrc)
-{
-  JSpan<const jdouble> nativeA{env, aSrc};
-  JSpan<const jdouble> nativeB{env, bSrc};
-
-  Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
-                                 Eigen::RowMajor>>
-      A{nativeA.data(), states, states};
-
-  Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
-                                 Eigen::RowMajor>>
-      B{nativeB.data(), states, inputs};
-
-  bool isStabilizable =
-      frc::IsStabilizable<Eigen::Dynamic, Eigen::Dynamic>(A, B);
-
-  return isStabilizable;
-}
-
-/*
- * Class:     edu_wpi_first_math_WPIMathJNI
- * Method:    fromPathweaverJson
- * Signature: (Ljava/lang/String;)[D
- */
-JNIEXPORT jdoubleArray JNICALL
-Java_edu_wpi_first_math_WPIMathJNI_fromPathweaverJson
-  (JNIEnv* env, jclass, jstring path)
-{
-  try {
-    auto trajectory =
-        frc::TrajectoryUtil::FromPathweaverJson(JStringRef{env, path}.c_str());
-    std::vector<double> elements = GetElementsFromTrajectory(trajectory);
-    return MakeJDoubleArray(env, elements);
-  } catch (std::exception& e) {
-    jclass cls = env->FindClass("java/io/IOException");
-    if (cls) {
-      env->ThrowNew(cls, e.what());
-    }
-    return nullptr;
-  }
-}
-
-/*
- * Class:     edu_wpi_first_math_WPIMathJNI
- * Method:    toPathweaverJson
- * Signature: ([DLjava/lang/String;)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_math_WPIMathJNI_toPathweaverJson
-  (JNIEnv* env, jclass, jdoubleArray elements, jstring path)
-{
-  try {
-    auto trajectory =
-        CreateTrajectoryFromElements(JSpan<const jdouble>{env, elements});
-    frc::TrajectoryUtil::ToPathweaverJson(trajectory,
-                                          JStringRef{env, path}.c_str());
-  } catch (std::exception& e) {
-    jclass cls = env->FindClass("java/io/IOException");
-    if (cls) {
-      env->ThrowNew(cls, e.what());
-    }
-  }
-}
-
-/*
- * Class:     edu_wpi_first_math_WPIMathJNI
- * Method:    deserializeTrajectory
- * Signature: (Ljava/lang/String;)[D
- */
-JNIEXPORT jdoubleArray JNICALL
-Java_edu_wpi_first_math_WPIMathJNI_deserializeTrajectory
-  (JNIEnv* env, jclass, jstring json)
-{
-  try {
-    auto trajectory = frc::TrajectoryUtil::DeserializeTrajectory(
-        JStringRef{env, json}.c_str());
-    std::vector<double> elements = GetElementsFromTrajectory(trajectory);
-    return MakeJDoubleArray(env, elements);
-  } catch (std::exception& e) {
-    jclass cls = env->FindClass(
-        "edu/wpi/first/math/trajectory/TrajectoryUtil$"
-        "TrajectorySerializationException");
-    if (cls) {
-      env->ThrowNew(cls, e.what());
-    }
-    return nullptr;
-  }
-}
-
-/*
- * Class:     edu_wpi_first_math_WPIMathJNI
- * Method:    serializeTrajectory
- * Signature: ([D)Ljava/lang/String;
- */
-JNIEXPORT jstring JNICALL
-Java_edu_wpi_first_math_WPIMathJNI_serializeTrajectory
-  (JNIEnv* env, jclass, jdoubleArray elements)
-{
-  try {
-    auto trajectory =
-        CreateTrajectoryFromElements(JSpan<const jdouble>{env, elements});
-    return MakeJString(env,
-                       frc::TrajectoryUtil::SerializeTrajectory(trajectory));
-  } catch (std::exception& e) {
-    jclass cls = env->FindClass(
-        "edu/wpi/first/math/trajectory/TrajectoryUtil$"
-        "TrajectorySerializationException");
-    if (cls) {
-      env->ThrowNew(cls, e.what());
-    }
-    return nullptr;
-  }
-}
-
-/*
- * Class:     edu_wpi_first_math_WPIMathJNI
- * Method:    rankUpdate
- * Signature: ([DI[DDZ)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_math_WPIMathJNI_rankUpdate
-  (JNIEnv* env, jclass, jdoubleArray mat, jint rows, jdoubleArray vec,
-   jdouble sigma, jboolean lowerTriangular)
-{
-  JSpan<jdouble> matBody{env, mat};
-  JSpan<const jdouble> vecBody{env, vec};
-
-  Eigen::Map<
-      Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>
-      L{matBody.data(), rows, rows};
-  Eigen::Map<const Eigen::Vector<double, Eigen::Dynamic>> v{vecBody.data(),
-                                                            rows};
-
-  if (lowerTriangular == JNI_TRUE) {
-    Eigen::internal::llt_inplace<double, Eigen::Lower>::rankUpdate(L, v, sigma);
-  } else {
-    Eigen::internal::llt_inplace<double, Eigen::Upper>::rankUpdate(L, v, sigma);
-  }
-}
-
-/*
- * Class:     edu_wpi_first_math_WPIMathJNI
- * Method:    solveFullPivHouseholderQr
- * Signature: ([DII[DII[D)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_math_WPIMathJNI_solveFullPivHouseholderQr
-  (JNIEnv* env, jclass, jdoubleArray A, jint Arows, jint Acols, jdoubleArray B,
-   jint Brows, jint Bcols, jdoubleArray dst)
-{
-  JSpan<const jdouble> nativeA{env, A};
-  JSpan<const jdouble> nativeB{env, B};
-
-  Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
-                                 Eigen::RowMajor>>
-      Amat{nativeA.data(), Arows, Acols};
-  Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
-                                 Eigen::RowMajor>>
-      Bmat{nativeB.data(), Brows, Bcols};
-
-  Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> Xmat =
-      Amat.fullPivHouseholderQr().solve(Bmat);
-
-  env->SetDoubleArrayRegion(dst, 0, Brows * Bcols, Xmat.data());
-}
-
-}  // extern "C"
diff --git a/wpimath/src/main/native/cpp/jni/WPIMathJNI_DARE.cpp b/wpimath/src/main/native/cpp/jni/WPIMathJNI_DARE.cpp
new file mode 100644
index 0000000000..e5c3ac5912
--- /dev/null
+++ b/wpimath/src/main/native/cpp/jni/WPIMathJNI_DARE.cpp
@@ -0,0 +1,177 @@
+// 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.
+
+#include <jni.h>
+
+#include <stdexcept>
+
+#include <wpi/jni_util.h>
+
+#include "WPIMathJNI_Exceptions.h"
+#include "edu_wpi_first_math_WPIMathJNI.h"
+#include "frc/DARE.h"
+
+using namespace wpi::java;
+
+extern "C" {
+
+/*
+ * Class:     edu_wpi_first_math_WPIMathJNI
+ * Method:    dareDetailABQR
+ * Signature: ([D[D[D[DII[D)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_math_WPIMathJNI_dareDetailABQR
+  (JNIEnv* env, jclass, jdoubleArray A, jdoubleArray B, jdoubleArray Q,
+   jdoubleArray R, jint states, jint inputs, jdoubleArray S)
+{
+  JSpan<const jdouble> nativeA{env, A};
+  JSpan<const jdouble> nativeB{env, B};
+  JSpan<const jdouble> nativeQ{env, Q};
+  JSpan<const jdouble> nativeR{env, R};
+
+  Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
+                                 Eigen::RowMajor>>
+      Amat{nativeA.data(), states, states};
+  Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
+                                 Eigen::RowMajor>>
+      Bmat{nativeB.data(), states, inputs};
+  Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
+                                 Eigen::RowMajor>>
+      Qmat{nativeQ.data(), states, states};
+  Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
+                                 Eigen::RowMajor>>
+      Rmat{nativeR.data(), inputs, inputs};
+
+  Eigen::MatrixXd RmatCopy{Rmat};
+  auto R_llt = RmatCopy.llt();
+
+  Eigen::MatrixXd result = frc::detail::DARE<Eigen::Dynamic, Eigen::Dynamic>(
+      Amat, Bmat, Qmat, R_llt);
+
+  env->SetDoubleArrayRegion(S, 0, states * states, result.data());
+}
+
+/*
+ * Class:     edu_wpi_first_math_WPIMathJNI
+ * Method:    dareDetailABQRN
+ * Signature: ([D[D[D[D[DII[D)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_math_WPIMathJNI_dareDetailABQRN
+  (JNIEnv* env, jclass, jdoubleArray A, jdoubleArray B, jdoubleArray Q,
+   jdoubleArray R, jdoubleArray N, jint states, jint inputs, jdoubleArray S)
+{
+  JSpan<const jdouble> nativeA{env, A};
+  JSpan<const jdouble> nativeB{env, B};
+  JSpan<const jdouble> nativeQ{env, Q};
+  JSpan<const jdouble> nativeR{env, R};
+  JSpan<const jdouble> nativeN{env, N};
+
+  Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
+                                 Eigen::RowMajor>>
+      Amat{nativeA.data(), states, states};
+  Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
+                                 Eigen::RowMajor>>
+      Bmat{nativeB.data(), states, inputs};
+  Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
+                                 Eigen::RowMajor>>
+      Qmat{nativeQ.data(), states, states};
+  Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
+                                 Eigen::RowMajor>>
+      Rmat{nativeR.data(), inputs, inputs};
+  Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
+                                 Eigen::RowMajor>>
+      Nmat{nativeN.data(), states, inputs};
+
+  Eigen::MatrixXd Rcopy{Rmat};
+  auto R_llt = Rcopy.llt();
+
+  Eigen::MatrixXd result = frc::detail::DARE<Eigen::Dynamic, Eigen::Dynamic>(
+      Amat, Bmat, Qmat, R_llt, Nmat);
+
+  env->SetDoubleArrayRegion(S, 0, states * states, result.data());
+}
+
+/*
+ * Class:     edu_wpi_first_math_WPIMathJNI
+ * Method:    dareABQR
+ * Signature: ([D[D[D[DII[D)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_math_WPIMathJNI_dareABQR
+  (JNIEnv* env, jclass, jdoubleArray A, jdoubleArray B, jdoubleArray Q,
+   jdoubleArray R, jint states, jint inputs, jdoubleArray S)
+{
+  JSpan<const jdouble> nativeA{env, A};
+  JSpan<const jdouble> nativeB{env, B};
+  JSpan<const jdouble> nativeQ{env, Q};
+  JSpan<const jdouble> nativeR{env, R};
+
+  Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
+                                 Eigen::RowMajor>>
+      Amat{nativeA.data(), states, states};
+  Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
+                                 Eigen::RowMajor>>
+      Bmat{nativeB.data(), states, inputs};
+  Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
+                                 Eigen::RowMajor>>
+      Qmat{nativeQ.data(), states, states};
+  Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
+                                 Eigen::RowMajor>>
+      Rmat{nativeR.data(), inputs, inputs};
+
+  try {
+    Eigen::MatrixXd result =
+        frc::DARE<Eigen::Dynamic, Eigen::Dynamic>(Amat, Bmat, Qmat, Rmat);
+
+    env->SetDoubleArrayRegion(S, 0, states * states, result.data());
+  } catch (const std::invalid_argument& e) {
+    illegalArgEx.Throw(env, e.what());
+  }
+}
+
+/*
+ * Class:     edu_wpi_first_math_WPIMathJNI
+ * Method:    dareABQRN
+ * Signature: ([D[D[D[D[DII[D)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_math_WPIMathJNI_dareABQRN
+  (JNIEnv* env, jclass, jdoubleArray A, jdoubleArray B, jdoubleArray Q,
+   jdoubleArray R, jdoubleArray N, jint states, jint inputs, jdoubleArray S)
+{
+  JSpan<const jdouble> nativeA{env, A};
+  JSpan<const jdouble> nativeB{env, B};
+  JSpan<const jdouble> nativeQ{env, Q};
+  JSpan<const jdouble> nativeR{env, R};
+  JSpan<const jdouble> nativeN{env, N};
+
+  Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
+                                 Eigen::RowMajor>>
+      Amat{nativeA.data(), states, states};
+  Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
+                                 Eigen::RowMajor>>
+      Bmat{nativeB.data(), states, inputs};
+  Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
+                                 Eigen::RowMajor>>
+      Qmat{nativeQ.data(), states, states};
+  Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
+                                 Eigen::RowMajor>>
+      Rmat{nativeR.data(), inputs, inputs};
+  Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
+                                 Eigen::RowMajor>>
+      Nmat{nativeN.data(), states, inputs};
+
+  try {
+    Eigen::MatrixXd result =
+        frc::DARE<Eigen::Dynamic, Eigen::Dynamic>(Amat, Bmat, Qmat, Rmat, Nmat);
+
+    env->SetDoubleArrayRegion(S, 0, states * states, result.data());
+  } catch (const std::invalid_argument& e) {
+    illegalArgEx.Throw(env, e.what());
+  }
+}
+
+}  // extern "C"
diff --git a/wpimath/src/main/native/cpp/jni/WPIMathJNI_Eigen.cpp b/wpimath/src/main/native/cpp/jni/WPIMathJNI_Eigen.cpp
new file mode 100644
index 0000000000..642a2bad69
--- /dev/null
+++ b/wpimath/src/main/native/cpp/jni/WPIMathJNI_Eigen.cpp
@@ -0,0 +1,112 @@
+// 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.
+
+#include <jni.h>
+
+#include <Eigen/Cholesky>
+#include <Eigen/Core>
+#include <Eigen/QR>
+#include <unsupported/Eigen/MatrixFunctions>
+#include <wpi/jni_util.h>
+
+#include "edu_wpi_first_math_WPIMathJNI.h"
+
+using namespace wpi::java;
+
+extern "C" {
+
+/*
+ * Class:     edu_wpi_first_math_WPIMathJNI
+ * Method:    exp
+ * Signature: ([DI[D)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_math_WPIMathJNI_exp
+  (JNIEnv* env, jclass, jdoubleArray src, jint rows, jdoubleArray dst)
+{
+  JSpan<const jdouble> arrayBody{env, src};
+
+  Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
+                                 Eigen::RowMajor>>
+      Amat{arrayBody.data(), rows, rows};
+  Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> Aexp =
+      Amat.exp();
+
+  env->SetDoubleArrayRegion(dst, 0, rows * rows, Aexp.data());
+}
+
+/*
+ * Class:     edu_wpi_first_math_WPIMathJNI
+ * Method:    pow
+ * Signature: ([DID[D)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_math_WPIMathJNI_pow
+  (JNIEnv* env, jclass, jdoubleArray src, jint rows, jdouble exponent,
+   jdoubleArray dst)
+{
+  JSpan<const jdouble> arrayBody{env, src};
+
+  Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
+                                 Eigen::RowMajor>>
+      Amat{arrayBody.data(), rows, rows};
+  Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> Apow =
+      Amat.pow(exponent);
+
+  env->SetDoubleArrayRegion(dst, 0, rows * rows, Apow.data());
+}
+
+/*
+ * Class:     edu_wpi_first_math_WPIMathJNI
+ * Method:    rankUpdate
+ * Signature: ([DI[DDZ)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_math_WPIMathJNI_rankUpdate
+  (JNIEnv* env, jclass, jdoubleArray mat, jint rows, jdoubleArray vec,
+   jdouble sigma, jboolean lowerTriangular)
+{
+  JSpan<jdouble> matBody{env, mat};
+  JSpan<const jdouble> vecBody{env, vec};
+
+  Eigen::Map<
+      Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>
+      L{matBody.data(), rows, rows};
+  Eigen::Map<const Eigen::Vector<double, Eigen::Dynamic>> v{vecBody.data(),
+                                                            rows};
+
+  if (lowerTriangular == JNI_TRUE) {
+    Eigen::internal::llt_inplace<double, Eigen::Lower>::rankUpdate(L, v, sigma);
+  } else {
+    Eigen::internal::llt_inplace<double, Eigen::Upper>::rankUpdate(L, v, sigma);
+  }
+}
+
+/*
+ * Class:     edu_wpi_first_math_WPIMathJNI
+ * Method:    solveFullPivHouseholderQr
+ * Signature: ([DII[DII[D)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_math_WPIMathJNI_solveFullPivHouseholderQr
+  (JNIEnv* env, jclass, jdoubleArray A, jint Arows, jint Acols, jdoubleArray B,
+   jint Brows, jint Bcols, jdoubleArray dst)
+{
+  JSpan<const jdouble> nativeA{env, A};
+  JSpan<const jdouble> nativeB{env, B};
+
+  Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
+                                 Eigen::RowMajor>>
+      Amat{nativeA.data(), Arows, Acols};
+  Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
+                                 Eigen::RowMajor>>
+      Bmat{nativeB.data(), Brows, Bcols};
+
+  Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> Xmat =
+      Amat.fullPivHouseholderQr().solve(Bmat);
+
+  env->SetDoubleArrayRegion(dst, 0, Brows * Bcols, Xmat.data());
+}
+
+}  // extern "C"
diff --git a/wpimath/src/main/native/cpp/jni/WPIMathJNI_Exceptions.cpp b/wpimath/src/main/native/cpp/jni/WPIMathJNI_Exceptions.cpp
new file mode 100644
index 0000000000..7f5a03f6fa
--- /dev/null
+++ b/wpimath/src/main/native/cpp/jni/WPIMathJNI_Exceptions.cpp
@@ -0,0 +1,56 @@
+// 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.
+
+#include "WPIMathJNI_Exceptions.h"
+
+#include <jni.h>
+
+using namespace wpi::java;
+
+//
+// Globals and load/unload
+//
+
+JException illegalArgEx;
+JException ioEx;
+JException trajectorySerializationEx;
+
+static const JExceptionInit exceptions[] = {
+    {"java/lang/IllegalArgumentException", &illegalArgEx},
+    {"java/io/IOException", &ioEx},
+    {"edu/wpi/first/math/trajectory/"
+     "TrajectoryUtil$TrajectorySerializationException",
+     &trajectorySerializationEx}};
+
+extern "C" {
+
+JNIEXPORT jint JNICALL JNI_OnLoad(JavaVM* vm, void* reserved) {
+  JNIEnv* env;
+  if (vm->GetEnv(reinterpret_cast<void**>(&env), JNI_VERSION_1_6) != JNI_OK) {
+    return JNI_ERR;
+  }
+
+  // Cache references to exceptions
+  for (auto& c : exceptions) {
+    *c.cls = JException(env, c.name);
+    if (!*c.cls) {
+      return JNI_ERR;
+    }
+  }
+
+  return JNI_VERSION_1_6;
+}
+
+JNIEXPORT void JNICALL JNI_OnUnload(JavaVM* vm, void* reserved) {
+  JNIEnv* env;
+  if (vm->GetEnv(reinterpret_cast<void**>(&env), JNI_VERSION_1_6) != JNI_OK) {
+    return;
+  }
+  // Delete global references
+  for (auto& c : exceptions) {
+    c.cls->free(env);
+  }
+}
+
+}  // extern "C"
diff --git a/wpimath/src/main/native/cpp/jni/WPIMathJNI_Exceptions.h b/wpimath/src/main/native/cpp/jni/WPIMathJNI_Exceptions.h
new file mode 100644
index 0000000000..cf0e1497e9
--- /dev/null
+++ b/wpimath/src/main/native/cpp/jni/WPIMathJNI_Exceptions.h
@@ -0,0 +1,11 @@
+// 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.
+
+#pragma once
+
+#include <wpi/jni_util.h>
+
+extern wpi::java::JException illegalArgEx;
+extern wpi::java::JException ioEx;
+extern wpi::java::JException trajectorySerializationEx;
diff --git a/wpimath/src/main/native/cpp/jni/WPIMathJNI_Pose3d.cpp b/wpimath/src/main/native/cpp/jni/WPIMathJNI_Pose3d.cpp
new file mode 100644
index 0000000000..f985231e54
--- /dev/null
+++ b/wpimath/src/main/native/cpp/jni/WPIMathJNI_Pose3d.cpp
@@ -0,0 +1,70 @@
+// 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.
+
+#include <jni.h>
+
+#include <wpi/jni_util.h>
+
+#include "edu_wpi_first_math_WPIMathJNI.h"
+#include "frc/geometry/Pose3d.h"
+
+using namespace wpi::java;
+
+extern "C" {
+
+/*
+ * Class:     edu_wpi_first_math_WPIMathJNI
+ * Method:    expPose3d
+ * Signature: (DDDDDDDDDDDDD)[D
+ */
+JNIEXPORT jdoubleArray JNICALL
+Java_edu_wpi_first_math_WPIMathJNI_expPose3d
+  (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,
+   jdouble twistRy, jdouble twistRz)
+{
+  frc::Pose3d pose{
+      units::meter_t{poseX}, units::meter_t{poseY}, units::meter_t{poseZ},
+      frc::Rotation3d{frc::Quaternion{poseQw, poseQx, poseQy, poseQz}}};
+  frc::Twist3d twist{units::meter_t{twistDx},  units::meter_t{twistDy},
+                     units::meter_t{twistDz},  units::radian_t{twistRx},
+                     units::radian_t{twistRy}, units::radian_t{twistRz}};
+
+  frc::Pose3d result = pose.Exp(twist);
+
+  const auto& resultQuaternion = result.Rotation().GetQuaternion();
+  return MakeJDoubleArray(
+      env, {{result.X().value(), result.Y().value(), result.Z().value(),
+             resultQuaternion.W(), resultQuaternion.X(), resultQuaternion.Y(),
+             resultQuaternion.Z()}});
+}
+
+/*
+ * Class:     edu_wpi_first_math_WPIMathJNI
+ * Method:    logPose3d
+ * Signature: (DDDDDDDDDDDDDD)[D
+ */
+JNIEXPORT jdoubleArray JNICALL
+Java_edu_wpi_first_math_WPIMathJNI_logPose3d
+  (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,
+   jdouble endQy, jdouble endQz)
+{
+  frc::Pose3d startPose{
+      units::meter_t{startX}, units::meter_t{startY}, units::meter_t{startZ},
+      frc::Rotation3d{frc::Quaternion{startQw, startQx, startQy, startQz}}};
+  frc::Pose3d endPose{
+      units::meter_t{endX}, units::meter_t{endY}, units::meter_t{endZ},
+      frc::Rotation3d{frc::Quaternion{endQw, endQx, endQy, endQz}}};
+
+  frc::Twist3d result = startPose.Log(endPose);
+
+  return MakeJDoubleArray(
+      env, {{result.dx.value(), result.dy.value(), result.dz.value(),
+             result.rx.value(), result.ry.value(), result.rz.value()}});
+}
+
+}  // extern "C"
diff --git a/wpimath/src/main/native/cpp/jni/WPIMathJNI_StateSpaceUtil.cpp b/wpimath/src/main/native/cpp/jni/WPIMathJNI_StateSpaceUtil.cpp
new file mode 100644
index 0000000000..dc3dfddf33
--- /dev/null
+++ b/wpimath/src/main/native/cpp/jni/WPIMathJNI_StateSpaceUtil.cpp
@@ -0,0 +1,44 @@
+// 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.
+
+#include <jni.h>
+
+#include <Eigen/Core>
+#include <wpi/jni_util.h>
+
+#include "edu_wpi_first_math_WPIMathJNI.h"
+#include "frc/StateSpaceUtil.h"
+
+using namespace wpi::java;
+
+extern "C" {
+
+/*
+ * Class:     edu_wpi_first_math_WPIMathJNI
+ * Method:    isStabilizable
+ * Signature: (II[D[D)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_math_WPIMathJNI_isStabilizable
+  (JNIEnv* env, jclass, jint states, jint inputs, jdoubleArray aSrc,
+   jdoubleArray bSrc)
+{
+  JSpan<const jdouble> nativeA{env, aSrc};
+  JSpan<const jdouble> nativeB{env, bSrc};
+
+  Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
+                                 Eigen::RowMajor>>
+      A{nativeA.data(), states, states};
+
+  Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
+                                 Eigen::RowMajor>>
+      B{nativeB.data(), states, inputs};
+
+  bool isStabilizable =
+      frc::IsStabilizable<Eigen::Dynamic, Eigen::Dynamic>(A, B);
+
+  return isStabilizable;
+}
+
+}  // extern "C"
diff --git a/wpimath/src/main/native/cpp/jni/WPIMathJNI_Trajectory.cpp b/wpimath/src/main/native/cpp/jni/WPIMathJNI_Trajectory.cpp
new file mode 100644
index 0000000000..3359da984c
--- /dev/null
+++ b/wpimath/src/main/native/cpp/jni/WPIMathJNI_Trajectory.cpp
@@ -0,0 +1,138 @@
+// 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.
+
+#include <jni.h>
+
+#include <exception>
+
+#include <wpi/jni_util.h>
+
+#include "WPIMathJNI_Exceptions.h"
+#include "edu_wpi_first_math_WPIMathJNI.h"
+#include "frc/trajectory/TrajectoryUtil.h"
+
+using namespace wpi::java;
+
+std::vector<double> GetElementsFromTrajectory(
+    const frc::Trajectory& trajectory) {
+  std::vector<double> elements;
+  elements.reserve(trajectory.States().size() * 7);
+
+  for (auto&& state : trajectory.States()) {
+    elements.push_back(state.t.value());
+    elements.push_back(state.velocity.value());
+    elements.push_back(state.acceleration.value());
+    elements.push_back(state.pose.X().value());
+    elements.push_back(state.pose.Y().value());
+    elements.push_back(state.pose.Rotation().Radians().value());
+    elements.push_back(state.curvature.value());
+  }
+
+  return elements;
+}
+
+frc::Trajectory CreateTrajectoryFromElements(std::span<const double> elements) {
+  // Make sure that the elements have the correct length.
+  assert(elements.size() % 7 == 0);
+
+  // Create a vector of states from the elements.
+  std::vector<frc::Trajectory::State> states;
+  states.reserve(elements.size() / 7);
+
+  for (size_t i = 0; i < elements.size(); i += 7) {
+    states.emplace_back(frc::Trajectory::State{
+        units::second_t{elements[i]},
+        units::meters_per_second_t{elements[i + 1]},
+        units::meters_per_second_squared_t{elements[i + 2]},
+        frc::Pose2d{units::meter_t{elements[i + 3]},
+                    units::meter_t{elements[i + 4]},
+                    units::radian_t{elements[i + 5]}},
+        units::curvature_t{elements[i + 6]}});
+  }
+
+  return frc::Trajectory(states);
+}
+
+extern "C" {
+
+/*
+ * Class:     edu_wpi_first_math_WPIMathJNI
+ * Method:    fromPathweaverJson
+ * Signature: (Ljava/lang/String;)[D
+ */
+JNIEXPORT jdoubleArray JNICALL
+Java_edu_wpi_first_math_WPIMathJNI_fromPathweaverJson
+  (JNIEnv* env, jclass, jstring path)
+{
+  try {
+    auto trajectory =
+        frc::TrajectoryUtil::FromPathweaverJson(JStringRef{env, path}.c_str());
+    std::vector<double> elements = GetElementsFromTrajectory(trajectory);
+    return MakeJDoubleArray(env, elements);
+  } catch (std::exception& e) {
+    ioEx.Throw(env, e.what());
+    return nullptr;
+  }
+}
+
+/*
+ * Class:     edu_wpi_first_math_WPIMathJNI
+ * Method:    toPathweaverJson
+ * Signature: ([DLjava/lang/String;)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_math_WPIMathJNI_toPathweaverJson
+  (JNIEnv* env, jclass, jdoubleArray elements, jstring path)
+{
+  try {
+    auto trajectory =
+        CreateTrajectoryFromElements(JSpan<const jdouble>{env, elements});
+    frc::TrajectoryUtil::ToPathweaverJson(trajectory,
+                                          JStringRef{env, path}.c_str());
+  } catch (std::exception& e) {
+    ioEx.Throw(env, e.what());
+  }
+}
+
+/*
+ * Class:     edu_wpi_first_math_WPIMathJNI
+ * Method:    deserializeTrajectory
+ * Signature: (Ljava/lang/String;)[D
+ */
+JNIEXPORT jdoubleArray JNICALL
+Java_edu_wpi_first_math_WPIMathJNI_deserializeTrajectory
+  (JNIEnv* env, jclass, jstring json)
+{
+  try {
+    auto trajectory = frc::TrajectoryUtil::DeserializeTrajectory(
+        JStringRef{env, json}.c_str());
+    std::vector<double> elements = GetElementsFromTrajectory(trajectory);
+    return MakeJDoubleArray(env, elements);
+  } catch (std::exception& e) {
+    trajectorySerializationEx.Throw(env, e.what());
+    return nullptr;
+  }
+}
+
+/*
+ * Class:     edu_wpi_first_math_WPIMathJNI
+ * Method:    serializeTrajectory
+ * Signature: ([D)Ljava/lang/String;
+ */
+JNIEXPORT jstring JNICALL
+Java_edu_wpi_first_math_WPIMathJNI_serializeTrajectory
+  (JNIEnv* env, jclass, jdoubleArray elements)
+{
+  try {
+    auto trajectory =
+        CreateTrajectoryFromElements(JSpan<const jdouble>{env, elements});
+    return MakeJString(env,
+                       frc::TrajectoryUtil::SerializeTrajectory(trajectory));
+  } catch (std::exception& e) {
+    trajectorySerializationEx.Throw(env, e.what());
+    return nullptr;
+  }
+}
+
+}  // extern "C"