Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-22 01:11:42 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpimath] Move math functionality into new wpimath library (#2629)

Browse Source 
The wpimath library is a new library designed to separate the reusable math functionality
from the common utility library (wpiutil) and the hardware-dependent library (wpilibc/j).

Package names / include file names were NOT changed to minimize breakage.  In a future year
it would be good to revamp these for a more uniform user experience and to reduce the risk
of accidental naming conflicts.

While theoretically all of this functionality could be placed into wpiutil, several pieces
of this library (e.g. DARE) are very time-consuming to compile, so it's nice to avoid this
expense for users who only want cscore or ntcore.  It also allows for easy future separation
of build tasks vs number of workers on memory-constrained machines.

This moves the following functionality from wpiutil into wpimath:
- Eigen
- ejml
- Drake
- DARE
- wpiutil.math package (Matrix etc)
- units

And the following functionality from wpilibc/j into wpimath:
- Geometry
- Kinematics
- Spline
- Trajectory
- LinearFilter
- MedianFilter
- Feed-forward controllers
This commit is contained in:
Peter Johnson
2020-08-06 23:57:39 -07:00
committed by GitHub
parent ad817d4f23
commit 42993b15c6
463 changed files with 1006 additions and 399 deletions
Download Patch File Download Diff File Expand all files Collapse all files

41
wpiutil/src/main/java/edu/wpi/first/wpiutil/ErrorMessages.java Normal file
View File

@@ -0,0 +1,41 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2018-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;
import static java.util.Objects.requireNonNull;
/**
* Utility class for common WPILib error messages.
*/
public final class ErrorMessages {
/**
* Utility class, so constructor is private.
*/
private ErrorMessages() {
throw new UnsupportedOperationException("This is a utility class!");
}
/**
* Requires that a parameter of a method not be null; prints an error message with
* helpful debugging instructions if the parameter is null.
*
* @param obj The parameter that must not be null.
* @param paramName The name of the parameter.
* @param methodName The name of the method.
*/
public static <T> T requireNonNullParam(T obj, String paramName, String methodName) {
return requireNonNull(obj,
"Parameter " + paramName + " in method " + methodName + " was null when it"
+ " should not have been! Check the stacktrace to find the responsible line of code - "
+ "usually, it is the first line of user-written code indicated in the stacktrace. "
+ "Make sure all objects passed to the method in question were properly initialized -"
+ " note that this may not be obvious if it is being called under "
+ "dynamically-changing conditions! Please do not seek additional technical assistance"
+ " without doing this first!");
}
}

56
wpiutil/src/main/java/edu/wpi/first/wpiutil/math/Drake.java
View File

@@ -1,56 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019-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 org.ejml.simple.SimpleMatrix;
public final class Drake {
private Drake() {
}
/**
* Solves the discrete alegebraic Riccati equation.
*
* @param A System matrix.
* @param B Input matrix.
* @param Q State cost matrix.
* @param R Input cost matrix.
* @return Solution of DARE.
*/
@SuppressWarnings({"LocalVariableName", "ParameterName"})
public static SimpleMatrix discreteAlgebraicRiccatiEquation(
SimpleMatrix A,
SimpleMatrix B,
SimpleMatrix Q,
SimpleMatrix R) {
var S = new SimpleMatrix(A.numRows(), A.numCols());
DrakeJNI.discreteAlgebraicRiccatiEquation(A.getDDRM().getData(), B.getDDRM().getData(),
Q.getDDRM().getData(), R.getDDRM().getData(), A.numCols(), B.numCols(),
S.getDDRM().getData());
return S;
}
/**
* Solves the discrete alegebraic Riccati equation.
*
* @param A System matrix.
* @param B Input matrix.
* @param Q State cost matrix.
* @param R Input cost matrix.
* @return Solution of DARE.
*/
@SuppressWarnings("ParameterName")
public static SimpleMatrix discreteAlgebraicRiccatiEquation(
Matrix A,
Matrix B,
Matrix Q,
Matrix R) {
return discreteAlgebraicRiccatiEquation(A.getStorage(), B.getStorage(), Q.getStorage(),
R.getStorage());
}
}

79
wpiutil/src/main/java/edu/wpi/first/wpiutil/math/DrakeJNI.java
View File

@@ -1,79 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019-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 DrakeJNI {
static boolean libraryLoaded = false;
static RuntimeLoader<DrakeJNI> loader = null;
static {
if (Helper.getExtractOnStaticLoad()) {
try {
loader = new RuntimeLoader<>("wpiutiljni", RuntimeLoader.getDefaultExtractionRoot(),
DrakeJNI.class);
loader.loadLibrary();
} catch (IOException 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;
}
loader = new RuntimeLoader<>("wpiutiljni", RuntimeLoader.getDefaultExtractionRoot(),
DrakeJNI.class);
loader.loadLibrary();
libraryLoaded = true;
}
/**
* 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 discreteAlgebraicRiccatiEquation(
double[] A,
double[] B,
double[] Q,
double[] R,
int states,
int inputs,
double[] S);
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);
}
}
}

50
wpiutil/src/main/java/edu/wpi/first/wpiutil/math/MatBuilder.java
View File

@@ -1,50 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 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.util.Objects;
import org.ejml.simple.SimpleMatrix;
/**
* A class for constructing arbitrary RxC matrices.
*
* @param <R> The number of rows of the desired matrix.
* @param <C> The number of columns of the desired matrix.
*/
public class MatBuilder<R extends Num, C extends Num> {
private final Nat<R> m_rows;
private final Nat<C> m_cols;
/**
* Fills the matrix with the given data, encoded in row major form.
* (The matrix is filled row by row, left to right with the given data).
*
* @param data The data to fill the matrix with.
* @return The constructed matrix.
*/
@SuppressWarnings("LineLength")
public final Matrix<R, C> fill(double... data) {
if (Objects.requireNonNull(data).length != this.m_rows.getNum() * this.m_cols.getNum()) {
throw new IllegalArgumentException("Invalid matrix data provided. Wanted " + this.m_rows.getNum()
+ " x " + this.m_cols.getNum() + " matrix, but got " + data.length + " elements");
} else {
return new Matrix<>(new SimpleMatrix(this.m_rows.getNum(), this.m_cols.getNum(), true, data));
}
}
/**
* Creates a new {@link MatBuilder} with the given dimensions.
* @param rows The number of rows of the matrix.
* @param cols The number of columns of the matrix.
*/
public MatBuilder(Nat<R> rows, Nat<C> cols) {
this.m_rows = Objects.requireNonNull(rows);
this.m_cols = Objects.requireNonNull(cols);
}
}

56
wpiutil/src/main/java/edu/wpi/first/wpiutil/math/MathUtil.java
View File

@@ -1,56 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019-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;
public final class MathUtil {
private MathUtil() {
throw new AssertionError("utility class");
}
/**
* Returns value clamped between low and high boundaries.
*
* @param value Value to clamp.
* @param low The lower boundary to which to clamp value.
* @param high The higher boundary to which to clamp value.
*/
public static int clamp(int value, int low, int high) {
return Math.max(low, Math.min(value, high));
}
/**
* Returns value clamped between low and high boundaries.
*
* @param value Value to clamp.
* @param low The lower boundary to which to clamp value.
* @param high The higher boundary to which to clamp value.
*/
public static double clamp(double value, double low, double high) {
return Math.max(low, Math.min(value, high));
}
/**
* Constrains theta to within the range (-pi, pi].
*
* @param theta The angle to normalize.
* @return The normalized angle.
*/
@SuppressWarnings("LocalVariableName")
public static double normalizeAngle(double theta) {
// Constraint theta to within (-3pi, pi)
int nPiPos = (int) ((theta + Math.PI) / 2.0 / Math.PI);
theta -= nPiPos * 2.0 * Math.PI;
// Cut off the bottom half of the above range to constrain within
// (-pi, pi]
int nPiNeg = (int) ((theta - Math.PI) / 2.0 / Math.PI);
theta -= nPiNeg * 2.0 * Math.PI;
return theta;
}
}

327
wpiutil/src/main/java/edu/wpi/first/wpiutil/math/Matrix.java
View File

@@ -1,327 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 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.util.Objects;
import org.ejml.dense.row.CommonOps_DDRM;
import org.ejml.dense.row.NormOps_DDRM;
import org.ejml.simple.SimpleMatrix;
/**
* A shape-safe wrapper over Efficient Java Matrix Library (EJML) matrices.
*
* <p>This class is intended to be used alongside the state space library.
*
* @param <R> The number of rows in this matrix.
* @param <C> The number of columns in this matrix.
*/
@SuppressWarnings("PMD.TooManyMethods")
public class Matrix<R extends Num, C extends Num> {
private final SimpleMatrix m_storage;
/**
* Gets the number of columns in this matrix.
*
* @return The number of columns, according to the internal storage.
*/
public final int getNumCols() {
return this.m_storage.numCols();
}
/**
* Gets the number of rows in this matrix.
*
* @return The number of rows, according to the internal storage.
*/
public final int getNumRows() {
return this.m_storage.numRows();
}
/**
* Get an element of this matrix.
*
* @param row The row of the element.
* @param col The column of the element.
* @return The element in this matrix at row,col.
*/
public final double get(int row, int col) {
return this.m_storage.get(row, col);
}
/**
* Sets the value at the given indices.
*
* @param row The row of the element.
* @param col The column of the element.
* @param value The value to insert at the given location.
*/
public final void set(int row, int col, double value) {
this.m_storage.set(row, col, value);
}
/**
* If a vector then a square matrix is returned
* if a matrix then a vector of diagonal elements is returned.
*
* @return Diagonal elements inside a vector or a square matrix with the same diagonal elements.
*/
public final Matrix<R, C> diag() {
return new Matrix<>(this.m_storage.diag());
}
/**
* Returns the largest element of this matrix.
*
* @return The largest element of this matrix.
*/
public final double maxInternal() {
return CommonOps_DDRM.elementMax(this.m_storage.getDDRM());
}
/**
* Returns the smallest element of this matrix.
*
* @return The smallest element of this matrix.
*/
public final double minInternal() {
return CommonOps_DDRM.elementMin(this.m_storage.getDDRM());
}
/**
* Calculates the mean of the elements in this matrix.
*
* @return The mean value of this matrix.
*/
public final double mean() {
return this.elementSum() / (double) this.m_storage.getNumElements();
}
/**
* Multiplies this matrix with another that has C rows.
*
* <p>As matrix multiplication is only defined if the number of columns
* in the first matrix matches the number of rows in the second,
* this operation will fail to compile under any other circumstances.
*
* @param other The other matrix to multiply by.
* @param <C2> The number of columns in the second matrix.
* @return The result of the matrix multiplication between this and the given matrix.
*/
public final <C2 extends Num> Matrix<R, C2> times(Matrix<C, C2> other) {
return new Matrix<>(this.m_storage.mult(other.m_storage));
}
/**
* Multiplies all the elements of this matrix by the given scalar.
*
* @param value The scalar value to multiply by.
* @return A new matrix with all the elements multiplied by the given value.
*/
public final Matrix<R, C> times(double value) {
return new Matrix<>(this.m_storage.scale(value));
}
/**
* <p>
* Returns a matrix which is the result of an element by element multiplication of 'this' and 'b'.
* c<sub>i,j</sub> = a<sub>i,j</sub>*b<sub>i,j</sub>
* </p>
*
* @param other A matrix.
* @return The element by element multiplication of 'this' and 'b'.
*/
public final Matrix<R, C> elementTimes(Matrix<R, C> other) {
return new Matrix<>(this.m_storage.elementMult(Objects.requireNonNull(other).m_storage));
}
/**
* Subtracts the given value from all the elements of this matrix.
*
* @param value The value to subtract.
* @return The resultant matrix.
*/
public final Matrix<R, C> minus(double value) {
return new Matrix<>(this.m_storage.minus(value));
}
/**
* Subtracts the given matrix from this matrix.
*
* @param value The matrix to subtract.
* @return The resultant matrix.
*/
public final Matrix<R, C> minus(Matrix<R, C> value) {
return new Matrix<>(this.m_storage.minus(Objects.requireNonNull(value).m_storage));
}
/**
* Adds the given value to all the elements of this matrix.
*
* @param value The value to add.
* @return The resultant matrix.
*/
public final Matrix<R, C> plus(double value) {
return new Matrix<>(this.m_storage.plus(value));
}
/**
* Adds the given matrix to this matrix.
*
* @param value The matrix to add.
* @return The resultant matrix.
*/
public final Matrix<R, C> plus(Matrix<R, C> value) {
return new Matrix<>(this.m_storage.plus(value.m_storage));
}
/**
* Divides all elements of this matrix by the given value.
*
* @param value The value to divide by.
* @return The resultant matrix.
*/
public final Matrix<R, C> div(int value) {
return new Matrix<>(this.m_storage.divide((double) value));
}
/**
* Divides all elements of this matrix by the given value.
*
* @param value The value to divide by.
* @return The resultant matrix.
*/
public final Matrix<R, C> div(double value) {
return new Matrix<>(this.m_storage.divide(value));
}
/**
* Calculates the transpose, M^T of this matrix.
*
* @return The tranpose matrix.
*/
public final Matrix<C, R> transpose() {
return new Matrix<>(this.m_storage.transpose());
}
/**
* Returns a copy of this matrix.
*
* @return A copy of this matrix.
*/
public final Matrix<R, C> copy() {
return new Matrix<>(this.m_storage.copy());
}
/**
* Returns the inverse matrix of this matrix.
*
* @return The inverse of this matrix.
* @throws org.ejml.data.SingularMatrixException If this matrix is non-invertable.
*/
public final Matrix<R, C> inv() {
return new Matrix<>(this.m_storage.invert());
}
/**
* Returns the determinant of this matrix.
*
* @return The determinant of this matrix.
*/
public final double det() {
return this.m_storage.determinant();
}
/**
* Computes the Frobenius normal of the matrix.<br>
* <br>
* normF = Sqrt{ &sum;<sub>i=1:m</sub> &sum;<sub>j=1:n</sub> { a<sub>ij</sub><sup>2</sup>} }
*
* @return The matrix's Frobenius normal.
*/
public final double normF() {
return this.m_storage.normF();
}
/**
* Computes the induced p = 1 matrix norm.<br>
* <br>
* ||A||<sub>1</sub>= max(j=1 to n; sum(i=1 to m; |a<sub>ij</sub>|))
*
* @return The norm.
*/
public final double normIndP1() {
return NormOps_DDRM.inducedP1(this.m_storage.getDDRM());
}
/**
* Computes the sum of all the elements in the matrix.
*
* @return Sum of all the elements.
*/
public final double elementSum() {
return this.m_storage.elementSum();
}
/**
* Computes the trace of the matrix.
*
* @return The trace of the matrix.
*/
public final double trace() {
return this.m_storage.trace();
}
/**
* Returns a matrix which is the result of an element by element power of 'this' and 'b':
* c<sub>i,j</sub> = a<sub>i,j</sub> ^ b.
*
* @param b Scalar
* @return The element by element power of 'this' and 'b'.
*/
@SuppressWarnings("ParameterName")
public final Matrix<R, C> epow(double b) {
return new Matrix<>(this.m_storage.elementPower(b));
}
/**
* Returns a matrix which is the result of an element by element power of 'this' and 'b':
* c<sub>i,j</sub> = a<sub>i,j</sub> ^ b.
*
* @param b Scalar.
* @return The element by element power of 'this' and 'b'.
*/
@SuppressWarnings("ParameterName")
public final Matrix<R, C> epow(int b) {
return new Matrix<>(this.m_storage.elementPower((double) b));
}
/**
* Returns the EJML {@link SimpleMatrix} backing this wrapper.
*
* @return The untyped EJML {@link SimpleMatrix}.
*/
public final SimpleMatrix getStorage() {
return this.m_storage;
}
/**
* Constructs a new matrix with the given storage.
* Caller should make sure that the provided generic bounds match the shape of the provided matrix
*
* @param storage The {@link SimpleMatrix} to back this value
*/
public Matrix(SimpleMatrix storage) {
this.m_storage = Objects.requireNonNull(storage);
}
}

83
wpiutil/src/main/java/edu/wpi/first/wpiutil/math/MatrixUtils.java
View File

@@ -1,83 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 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.util.Objects;
import org.ejml.simple.SimpleMatrix;
import edu.wpi.first.wpiutil.math.numbers.N1;
public final class MatrixUtils {
private MatrixUtils() {
throw new AssertionError("utility class");
}
/**
* Creates a new matrix of zeros.
*
* @param rows The number of rows in the matrix.
* @param cols The number of columns in the matrix.
* @param <R> The number of rows in the matrix as a generic.
* @param <C> The number of columns in the matrix as a generic.
* @return An RxC matrix filled with zeros.
*/
@SuppressWarnings("LineLength")
public static <R extends Num, C extends Num> Matrix<R, C> zeros(Nat<R> rows, Nat<C> cols) {
return new Matrix<>(
new SimpleMatrix(Objects.requireNonNull(rows).getNum(), Objects.requireNonNull(cols).getNum()));
}
/**
* Creates a new vector of zeros.
*
* @param nums The size of the desired vector.
* @param <N> The size of the desired vector as a generic.
* @return A vector of size N filled with zeros.
*/
public static <N extends Num> Matrix<N, N1> zeros(Nat<N> nums) {
return new Matrix<>(new SimpleMatrix(Objects.requireNonNull(nums).getNum(), 1));
}
/**
* Creates the identity matrix of the given dimension.
*
* @param dim The dimension of the desired matrix.
* @param <D> The dimension of the desired matrix as a generic.
* @return The DxD identity matrix.
*/
public static <D extends Num> Matrix<D, D> eye(Nat<D> dim) {
return new Matrix<>(SimpleMatrix.identity(Objects.requireNonNull(dim).getNum()));
}
/**
* Entrypoint to the MatBuilder class for creation
* of custom matrices with the given dimensions and contents.
*
* @param rows The number of rows of the desired matrix.
* @param cols The number of columns of the desired matrix.
* @param <R> The number of rows of the desired matrix as a generic.
* @param <C> The number of columns of the desired matrix as a generic.
* @return A builder to construct the matrix.
*/
public static <R extends Num, C extends Num> MatBuilder<R, C> mat(Nat<R> rows, Nat<C> cols) {
return new MatBuilder<>(rows, cols);
}
/**
* Entrypoint to the VecBuilder class for creation
* of custom vectors with the given size and contents.
*
* @param dim The dimension of the vector.
* @param <D> The dimension of the vector as a generic.
* @return A builder to construct the vector.
*/
public static <D extends Num> VecBuilder<D> vec(Nat<D> dim) {
return new VecBuilder<>(dim);
}
}

20
wpiutil/src/main/java/edu/wpi/first/wpiutil/math/Num.java
View File

@@ -1,20 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 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;
/**
* A number expressed as a java class.
*/
public abstract class Num {
/**
* The number this is backing.
*
* @return The number represented by this class.
*/
public abstract int getNum();
}

161
wpiutil/src/main/java/edu/wpi/first/wpiutil/math/SimpleMatrixUtils.java
View File

@@ -1,161 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 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.util.function.BiFunction;
import org.ejml.dense.row.NormOps_DDRM;
import org.ejml.simple.SimpleBase;
import org.ejml.simple.SimpleMatrix;
public class SimpleMatrixUtils {
private SimpleMatrixUtils() {}
/**
* Compute the matrix exponential, e^M of the given matrix.
*
* @param matrix The matrix to compute the exponential of.
* @return The resultant matrix.
*/
@SuppressWarnings({"LocalVariableName", "LineLength"})
public static SimpleMatrix expm(SimpleMatrix matrix) {
BiFunction<SimpleMatrix, SimpleMatrix, SimpleMatrix> solveProvider = SimpleBase::solve;
SimpleMatrix A = matrix;
double A_L1 = NormOps_DDRM.inducedP1(matrix.getDDRM());
int n_squarings = 0;
if (A_L1 < 1.495585217958292e-002) {
Pair<SimpleMatrix, SimpleMatrix> pair = _pade3(A);
return dispatchPade(pair.getFirst(), pair.getSecond(), n_squarings, solveProvider);
} else if (A_L1 < 2.539398330063230e-001) {
Pair<SimpleMatrix, SimpleMatrix> pair = _pade5(A);
return dispatchPade(pair.getFirst(), pair.getSecond(), n_squarings, solveProvider);
} else if (A_L1 < 9.504178996162932e-001) {
Pair<SimpleMatrix, SimpleMatrix> pair = _pade7(A);
return dispatchPade(pair.getFirst(), pair.getSecond(), n_squarings, solveProvider);
} else if (A_L1 < 2.097847961257068e+000) {
Pair<SimpleMatrix, SimpleMatrix> pair = _pade9(A);
return dispatchPade(pair.getFirst(), pair.getSecond(), n_squarings, solveProvider);
} else {
double maxNorm = 5.371920351148152;
double log = Math.log(A_L1 / maxNorm) / Math.log(2); // logb(2, arg)
n_squarings = (int) Math.max(0, Math.ceil(log));
A = A.divide(Math.pow(2.0, n_squarings));
Pair<SimpleMatrix, SimpleMatrix> pair = _pade13(A);
return dispatchPade(pair.getFirst(), pair.getSecond(), n_squarings, solveProvider);
}
}
@SuppressWarnings({"LocalVariableName", "ParameterName", "LineLength"})
private static SimpleMatrix dispatchPade(SimpleMatrix U, SimpleMatrix V,
int nSquarings, BiFunction<SimpleMatrix, SimpleMatrix, SimpleMatrix> solveProvider) {
SimpleMatrix P = U.plus(V);
SimpleMatrix Q = U.negative().plus(V);
SimpleMatrix R = solveProvider.apply(Q, P);
for (int i = 0; i < nSquarings; i++) {
R = R.mult(R);
}
return R;
}
@SuppressWarnings({"MethodName", "LocalVariableName", "ParameterName"})
private static Pair<SimpleMatrix, SimpleMatrix> _pade3(SimpleMatrix A) {
double[] b = new double[]{120, 60, 12, 1};
SimpleMatrix ident = eye(A.numRows(), A.numCols());
SimpleMatrix A2 = A.mult(A);
SimpleMatrix U = A.mult(A2.mult(ident.scale(b[1]).plus(b[3])));
SimpleMatrix V = A2.scale(b[2]).plus(ident.scale(b[0]));
return new Pair<>(U, V);
}
@SuppressWarnings({"MethodName", "LocalVariableName", "ParameterName"})
private static Pair<SimpleMatrix, SimpleMatrix> _pade5(SimpleMatrix A) {
double[] b = new double[]{30240, 15120, 3360, 420, 30, 1};
SimpleMatrix ident = eye(A.numRows(), A.numCols());
SimpleMatrix A2 = A.mult(A);
SimpleMatrix A4 = A2.mult(A2);
SimpleMatrix U = A.mult(A4.scale(b[5]).plus(A2.scale(b[3])).plus(ident.scale(b[1])));
SimpleMatrix V = A4.scale(b[4]).plus(A2.scale(b[2])).plus(ident.scale(b[0]));
return new Pair<>(U, V);
}
@SuppressWarnings({"MethodName", "LocalVariableName", "LineLength", "ParameterName"})
private static Pair<SimpleMatrix, SimpleMatrix> _pade7(SimpleMatrix A) {
double[] b = new double[]{17297280, 8648640, 1995840, 277200, 25200, 1512, 56, 1};
SimpleMatrix ident = eye(A.numRows(), A.numCols());
SimpleMatrix A2 = A.mult(A);
SimpleMatrix A4 = A2.mult(A2);
SimpleMatrix A6 = A4.mult(A2);
SimpleMatrix U = A.mult(A6.scale(b[7]).plus(A4.scale(b[5])).plus(A2.scale(b[3])).plus(ident.scale(b[1])));
SimpleMatrix V = A6.scale(b[6]).plus(A4.scale(b[4])).plus(A2.scale(b[2])).plus(ident.scale(b[0]));
return new Pair<>(U, V);
}
@SuppressWarnings({"MethodName", "LocalVariableName", "ParameterName", "LineLength"})
private static Pair<SimpleMatrix, SimpleMatrix> _pade9(SimpleMatrix A) {
double[] b = new double[]{17643225600.0, 8821612800.0, 2075673600, 302702400, 30270240,
2162160, 110880, 3960, 90, 1};
SimpleMatrix ident = eye(A.numRows(), A.numCols());
SimpleMatrix A2 = A.mult(A);
SimpleMatrix A4 = A2.mult(A2);
SimpleMatrix A6 = A4.mult(A2);
SimpleMatrix A8 = A6.mult(A2);
SimpleMatrix U = A.mult(A8.scale(b[9]).plus(A6.scale(b[7])).plus(A4.scale(b[5])).plus(A2.scale(b[3])).plus(ident.scale(b[1])));
SimpleMatrix V = A8.scale(b[8]).plus(A6.scale(b[6])).plus(A4.scale(b[4])).plus(A2.scale(b[2])).plus(ident.scale(b[0]));
return new Pair<>(U, V);
}
@SuppressWarnings({"MethodName", "LocalVariableName", "LineLength", "ParameterName"})
private static Pair<SimpleMatrix, SimpleMatrix> _pade13(SimpleMatrix A) {
double[] b = new double[]{64764752532480000.0, 32382376266240000.0, 7771770303897600.0,
1187353796428800.0, 129060195264000.0, 10559470521600.0, 670442572800.0,
33522128640.0, 1323241920, 40840800, 960960, 16380, 182, 1};
SimpleMatrix ident = eye(A.numRows(), A.numCols());
SimpleMatrix A2 = A.mult(A);
SimpleMatrix A4 = A2.mult(A2);
SimpleMatrix A6 = A4.mult(A2);
SimpleMatrix U = A.mult(A6.scale(b[13]).plus(A4.scale(b[11])).plus(A2.scale(b[9])).plus(A6.scale(b[7])).plus(A4.scale(b[5])).plus(A2.scale(b[3])).plus(ident.scale(b[1])));
SimpleMatrix V = A6.mult(A6.scale(b[12]).plus(A4.scale(b[10])).plus(A2.scale(b[8]))).plus(A6.scale(b[6]).plus(A4.scale(b[4])).plus(A2.scale(b[2])).plus(ident.scale(b[0])));
return new Pair<>(U, V);
}
private static SimpleMatrix eye(int rows, int cols) {
return SimpleMatrix.identity(Math.min(rows, cols));
}
private static class Pair<A, B> {
private final A m_first;
private final B m_second;
Pair(A first, B second) {
m_first = first;
m_second = second;
}
public A getFirst() {
return m_first;
}
public B getSecond() {
return m_second;
}
}
}

21
wpiutil/src/main/java/edu/wpi/first/wpiutil/math/VecBuilder.java
View File

@@ -1,21 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 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 edu.wpi.first.wpiutil.math.numbers.N1;
/**
* A specialization of {@link MatBuilder} for constructing vectors (Nx1 matrices).
*
* @param <N> The dimension of the vector to be constructed.
*/
public class VecBuilder<N extends Num> extends MatBuilder<N, N1> {
public VecBuilder(Nat<N> rows) {
super(rows, Nat.N1());
}
}