001// Copyright (c) FIRST and other WPILib contributors.
002// Open Source Software; you can modify and/or share it under the terms of
003// the WPILib BSD license file in the root directory of this project.
004
005package edu.wpi.first.math;
006
007import edu.wpi.first.util.RuntimeLoader;
008import java.io.IOException;
009import java.util.concurrent.atomic.AtomicBoolean;
010
011public final class WPIMathJNI {
012  static boolean libraryLoaded = false;
013  static RuntimeLoader<WPIMathJNI> loader = null;
014
015  static {
016    if (Helper.getExtractOnStaticLoad()) {
017      try {
018        loader =
019            new RuntimeLoader<>(
020                "wpimathjni", RuntimeLoader.getDefaultExtractionRoot(), WPIMathJNI.class);
021        loader.loadLibrary();
022      } catch (IOException ex) {
023        ex.printStackTrace();
024        System.exit(1);
025      }
026      libraryLoaded = true;
027    }
028  }
029
030  /**
031   * Force load the library.
032   *
033   * @throws IOException If the library could not be loaded or found.
034   */
035  public static synchronized void forceLoad() throws IOException {
036    if (libraryLoaded) {
037      return;
038    }
039    loader =
040        new RuntimeLoader<>(
041            "wpimathjni", RuntimeLoader.getDefaultExtractionRoot(), WPIMathJNI.class);
042    loader.loadLibrary();
043    libraryLoaded = true;
044  }
045
046  /**
047   * Solves the discrete alegebraic Riccati equation.
048   *
049   * @param A Array containing elements of A in row-major order.
050   * @param B Array containing elements of B in row-major order.
051   * @param Q Array containing elements of Q in row-major order.
052   * @param R Array containing elements of R in row-major order.
053   * @param states Number of states in A matrix.
054   * @param inputs Number of inputs in B matrix.
055   * @param S Array storage for DARE solution.
056   */
057  public static native void discreteAlgebraicRiccatiEquation(
058      double[] A, double[] B, double[] Q, double[] R, int states, int inputs, double[] S);
059
060  /**
061   * Computes the matrix exp.
062   *
063   * @param src Array of elements of the matrix to be exponentiated.
064   * @param rows How many rows there are.
065   * @param dst Array where the result will be stored.
066   */
067  public static native void exp(double[] src, int rows, double[] dst);
068
069  /**
070   * Computes the matrix pow.
071   *
072   * @param src Array of elements of the matrix to be raised to a power.
073   * @param rows How many rows there are.
074   * @param exponent The exponent.
075   * @param dst Array where the result will be stored.
076   */
077  public static native void pow(double[] src, int rows, double exponent, double[] dst);
078
079  /**
080   * Returns true if (A, B) is a stabilizable pair.
081   *
082   * <p>(A, B) is stabilizable if and only if the uncontrollable eigenvalues of A, if any, have
083   * absolute values less than one, where an eigenvalue is uncontrollable if rank(lambda * I - A, B)
084   * &lt; n where n is the number of states.
085   *
086   * @param states the number of states of the system.
087   * @param inputs the number of inputs to the system.
088   * @param A System matrix.
089   * @param B Input matrix.
090   * @return If the system is stabilizable.
091   */
092  public static native boolean isStabilizable(int states, int inputs, double[] A, double[] B);
093
094  /**
095   * Loads a Pathweaver JSON.
096   *
097   * @param path The path to the JSON.
098   * @return A double array with the trajectory states from the JSON.
099   * @throws IOException if the JSON could not be read.
100   */
101  public static native double[] fromPathweaverJson(String path) throws IOException;
102
103  /**
104   * Converts a trajectory into a Pathweaver JSON and saves it.
105   *
106   * @param elements The elements of the trajectory.
107   * @param path The location to save the JSON to.
108   * @throws IOException if the JSON could not be written.
109   */
110  public static native void toPathweaverJson(double[] elements, String path) throws IOException;
111
112  /**
113   * Deserializes a trajectory JSON into a double[] of trajectory elements.
114   *
115   * @param json The JSON containing the serialized trajectory.
116   * @return A double array with the trajectory states.
117   */
118  public static native double[] deserializeTrajectory(String json);
119
120  /**
121   * Serializes the trajectory into a JSON string.
122   *
123   * @param elements The elements of the trajectory.
124   * @return A JSON containing the serialized trajectory.
125   */
126  public static native String serializeTrajectory(double[] elements);
127
128  public static class Helper {
129    private static AtomicBoolean extractOnStaticLoad = new AtomicBoolean(true);
130
131    public static boolean getExtractOnStaticLoad() {
132      return extractOnStaticLoad.get();
133    }
134
135    public static void setExtractOnStaticLoad(boolean load) {
136      extractOnStaticLoad.set(load);
137    }
138  }
139}