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  /**
129   * Performs an inplace rank one update (or downdate) of an upper triangular Cholesky decomposition
130   * matrix.
131   *
132   * @param mat Array of elements of the matrix to be updated.
133   * @param lowerTriangular Whether mat is lower triangular.
134   * @param rows How many rows there are.
135   * @param vec Vector to use for the rank update.
136   * @param sigma Sigma value to use for the rank update.
137   */
138  public static native void rankUpdate(
139      double[] mat, int rows, double[] vec, double sigma, boolean lowerTriangular);
140
141  public static class Helper {
142    private static AtomicBoolean extractOnStaticLoad = new AtomicBoolean(true);
143
144    public static boolean getExtractOnStaticLoad() {
145      return extractOnStaticLoad.get();
146    }
147
148    public static void setExtractOnStaticLoad(boolean load) {
149      extractOnStaticLoad.set(load);
150    }
151  }
152
153  /**
154   * Solves the least-squares problem Ax=B using a QR decomposition with full pivoting.
155   *
156   * @param A Array of elements of the A matrix.
157   * @param Arows Number of rows of the A matrix.
158   * @param Acols Number of rows of the A matrix.
159   * @param B Array of elements of the B matrix.
160   * @param Brows Number of rows of the B matrix.
161   * @param Bcols Number of rows of the B matrix.
162   * @param dst Array to store solution in. If A is m-n and B is m-p, dst is n-p.
163   */
164  public static native void solveFullPivHouseholderQr(
165      double[] A, int Arows, int Acols, double[] B, int Brows, int Bcols, double[] dst);
166}