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 d9d8fc882e..30984ac3b1 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/WPIMathJNI.java +++ b/wpimath/src/main/java/edu/wpi/first/math/WPIMathJNI.java @@ -65,6 +65,30 @@ public final class WPIMathJNI { int inputs, double[] S); + /** + * 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); + + /** + * Returns true if (A, B) is a stabilizable pair. + * + *

(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) < n where n is 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); + public static class Helper { private static AtomicBoolean extractOnStaticLoad = new AtomicBoolean(true); diff --git a/wpimath/src/main/java/edu/wpi/first/wpilibj/math/StateSpaceUtil.java b/wpimath/src/main/java/edu/wpi/first/wpilibj/math/StateSpaceUtil.java index c37d9182c5..1df0f43469 100644 --- a/wpimath/src/main/java/edu/wpi/first/wpilibj/math/StateSpaceUtil.java +++ b/wpimath/src/main/java/edu/wpi/first/wpilibj/math/StateSpaceUtil.java @@ -11,13 +11,13 @@ import java.util.Random; import org.ejml.simple.SimpleMatrix; +import edu.wpi.first.math.WPIMathJNI; import edu.wpi.first.wpilibj.geometry.Pose2d; import edu.wpi.first.wpiutil.math.MathUtil; import edu.wpi.first.wpiutil.math.Matrix; import edu.wpi.first.wpiutil.math.Nat; import edu.wpi.first.wpiutil.math.Num; import edu.wpi.first.wpiutil.math.VecBuilder; -import edu.wpi.first.wpiutil.math.WPIMathJNI; import edu.wpi.first.wpiutil.math.numbers.N1; import edu.wpi.first.wpiutil.math.numbers.N3; diff --git a/wpimath/src/main/java/edu/wpi/first/wpiutil/math/Matrix.java b/wpimath/src/main/java/edu/wpi/first/wpiutil/math/Matrix.java index face6ea924..6c64d0def8 100644 --- a/wpimath/src/main/java/edu/wpi/first/wpiutil/math/Matrix.java +++ b/wpimath/src/main/java/edu/wpi/first/wpiutil/math/Matrix.java @@ -9,6 +9,7 @@ package edu.wpi.first.wpiutil.math; import java.util.Objects; + import org.ejml.MatrixDimensionException; import org.ejml.data.DMatrixRMaj; import org.ejml.dense.row.CommonOps_DDRM; @@ -18,6 +19,7 @@ import org.ejml.dense.row.factory.DecompositionFactory_DDRM; import org.ejml.interfaces.decomposition.CholeskyDecomposition_F64; import org.ejml.simple.SimpleMatrix; +import edu.wpi.first.math.WPIMathJNI; import edu.wpi.first.wpiutil.math.numbers.N1; /** diff --git a/wpimath/src/main/java/edu/wpi/first/wpiutil/math/SimpleMatrixUtils.java b/wpimath/src/main/java/edu/wpi/first/wpiutil/math/SimpleMatrixUtils.java index ed53d12500..a6fa072298 100644 --- a/wpimath/src/main/java/edu/wpi/first/wpiutil/math/SimpleMatrixUtils.java +++ b/wpimath/src/main/java/edu/wpi/first/wpiutil/math/SimpleMatrixUtils.java @@ -16,6 +16,8 @@ import org.ejml.interfaces.decomposition.CholeskyDecomposition_F64; import org.ejml.simple.SimpleBase; import org.ejml.simple.SimpleMatrix; +import edu.wpi.first.math.WPIMathJNI; + @SuppressWarnings("PMD.TooManyMethods") public final class SimpleMatrixUtils { private SimpleMatrixUtils() { diff --git a/wpimath/src/main/java/edu/wpi/first/wpiutil/math/WPIMathJNI.java b/wpimath/src/main/java/edu/wpi/first/wpiutil/math/WPIMathJNI.java deleted file mode 100644 index 0d47caa467..0000000000 --- a/wpimath/src/main/java/edu/wpi/first/wpiutil/math/WPIMathJNI.java +++ /dev/null @@ -1,79 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2020 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -package edu.wpi.first.wpiutil.math; - -import java.io.IOException; -import java.util.concurrent.atomic.AtomicBoolean; - -import edu.wpi.first.wpiutil.RuntimeLoader; - -public final class WPIMathJNI { - static boolean libraryLoaded = false; - static RuntimeLoader loader = null; - - public static class Helper { - private static AtomicBoolean extractOnStaticLoad = new AtomicBoolean(true); - - public static boolean getExtractOnStaticLoad() { - return extractOnStaticLoad.get(); - } - - public static void setExtractOnStaticLoad(boolean load) { - extractOnStaticLoad.set(load); - } - } - - static { - if (Helper.getExtractOnStaticLoad()) { - try { - loader = new RuntimeLoader<>("wpimathjni", RuntimeLoader.getDefaultExtractionRoot(), WPIMathJNI.class); - loader.loadLibrary(); - } catch (IOException ex) { - ex.printStackTrace(); - System.exit(1); - } - libraryLoaded = true; - } - } - - /** - * Force load the library. - */ - public static synchronized void forceLoad() throws IOException { - if (libraryLoaded) { - return; - } - loader = new RuntimeLoader<>("wpiutiljni", RuntimeLoader.getDefaultExtractionRoot(), WPIMathJNI.class); - loader.loadLibrary(); - 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); - - /** - * Returns true if (A, B) is a stabilizable pair. - * - *

(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) < n where n is 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); -} diff --git a/wpimath/src/main/native/cpp/jni/WPIMathJNI.cpp b/wpimath/src/main/native/cpp/jni/WPIMathJNI.cpp index 613c6cd3af..26d57e2ab0 100644 --- a/wpimath/src/main/native/cpp/jni/WPIMathJNI.cpp +++ b/wpimath/src/main/native/cpp/jni/WPIMathJNI.cpp @@ -85,12 +85,12 @@ Java_edu_wpi_first_math_WPIMathJNI_discreteAlgebraicRiccatiEquation } /* - * Class: edu_wpi_first_wpiutil_math_WPIMathJNI + * Class: edu_wpi_first_math_WPIMathJNI * Method: exp * Signature: ([DI[D)V */ JNIEXPORT void JNICALL -Java_edu_wpi_first_wpiutil_math_WPIMathJNI_exp +Java_edu_wpi_first_math_WPIMathJNI_exp (JNIEnv* env, jclass, jdoubleArray src, jint rows, jdoubleArray dst) { jdouble* arrayBody = env->GetDoubleArrayElements(src, nullptr); @@ -106,12 +106,12 @@ Java_edu_wpi_first_wpiutil_math_WPIMathJNI_exp } /* - * Class: edu_wpi_first_wpiutil_math_WPIMathJNI + * Class: edu_wpi_first_math_WPIMathJNI * Method: isStabilizable * Signature: (II[D[D)Z */ JNIEXPORT jboolean JNICALL -Java_edu_wpi_first_wpiutil_math_WPIMathJNI_isStabilizable +Java_edu_wpi_first_math_WPIMathJNI_isStabilizable (JNIEnv* env, jclass, jint states, jint inputs, jdoubleArray aSrc, jdoubleArray bSrc) {