diff --git a/wpiutil/CMakeLists.txt b/wpiutil/CMakeLists.txt index 52d5260579..ef3c5d6b02 100644 --- a/wpiutil/CMakeLists.txt +++ b/wpiutil/CMakeLists.txt @@ -40,7 +40,9 @@ if (NOT WITHOUT_JAVA) set(CMAKE_JAVA_INCLUDE_PATH wpiutil.jar ${EJML_JARS}) - file(GLOB_RECURSE JAVA_SOURCES src/main/java/*.java) + execute_process(COMMAND python3 ${CMAKE_SOURCE_DIR}/wpiutil/generate_numbers.py ${CMAKE_BINARY_DIR}/wpiutil) + + file(GLOB_RECURSE JAVA_SOURCES src/main/java/*.java ${CMAKE_BINARY_DIR}/wpiutil/generated/*.java) add_jar(wpiutil_jar ${JAVA_SOURCES} INCLUDE_JARS ${EJML_JARS} OUTPUT_NAME wpiutil) get_property(WPIUTIL_JAR_FILE TARGET wpiutil_jar PROPERTY JAR_FILE) diff --git a/wpiutil/build.gradle b/wpiutil/build.gradle index 6e1b35e08c..7a9e7ac95c 100644 --- a/wpiutil/build.gradle +++ b/wpiutil/build.gradle @@ -240,3 +240,55 @@ model { dependencies { compile "org.ejml:ejml-simple:0.38" } + +def wpilibNumberFileInput = file("src/generate/GenericNumber.java.in") +def natFileInput = file("src/generate/Nat.java.in") +def natGetterInput = file("src/generate/NatGetter.java.in") +def wpilibNumberFileOutputDir = file("$buildDir/generated/java/edu/wpi/first/wpiutil/math/numbers") +def wpilibNatFileOutput = file("$buildDir/generated/java/edu/wpi/first/wpiutil/math/Nat.java") +def maxNum = 20 + +task generateNumbers() { + description = "Generates generic number classes from template" + group = "WPILib" + + inputs.file wpilibNumberFileInput + + doLast { + if(wpilibNumberFileOutputDir.exists()) { + wpilibNumberFileOutputDir.delete() + } + wpilibNumberFileOutputDir.mkdirs() + + for(i in 0..maxNum) { + def outputFile = new File(wpilibNumberFileOutputDir, "N${i}.java") + def read = wpilibNumberFileInput.text.replace('${num}', i.toString()) + outputFile.write(read) + } + } +} + +task generateNat() { + description = "Generates Nat.java" + group = "WPILib" + inputs.files([natFileInput, natGetterInput]) + dependsOn generateNumbers + + doLast { + if(wpilibNatFileOutput.exists()) { + wpilibNatFileOutput.delete() + } + + def template = natFileInput.text + "\n" + for(i in 0..maxNum) { + template += natGetterInput.text.replace('${num}', i.toString()) + "\n" + } + template += "}\n" // Close the class body + + wpilibNatFileOutput.write(template) + } +} + +sourceSets.main.java.srcDir "${buildDir}/generated/java" +compileJava.dependsOn generateNumbers +compileJava.dependsOn generateNat diff --git a/wpiutil/generate_numbers.py b/wpiutil/generate_numbers.py new file mode 100644 index 0000000000..91f2bb69d9 --- /dev/null +++ b/wpiutil/generate_numbers.py @@ -0,0 +1,37 @@ +import os +import shutil +import sys + +MAX_NUM = 20 + +dirname, _ = os.path.split(os.path.abspath(__file__)) +cmake_binary_dir = sys.argv[1] + +with open(f"{dirname}/src/generate/GenericNumber.java.in", "r") as templateFile: + template = templateFile.read() + rootPath = f"{cmake_binary_dir}/generated/main/java/edu/wpi/first/wpiutil/math/numbers" + + if os.path.exists(rootPath): + shutil.rmtree(rootPath) + os.makedirs(rootPath) + + for i in range(MAX_NUM + 1): + with open(f"{rootPath}/N{i}.java", "w") as f: + f.write(template.replace("${num}", str(i))) + +with open(f"{dirname}/src/generate/Nat.java.in", "r") as templateFile: + template = templateFile.read() + outputPath = f"{cmake_binary_dir}/generated/main/java/edu/wpi/first/wpiutil/math/Nat.java" + with open(f"{dirname}/src/generate/NatGetter.java.in", "r") as getterFile: + getter = getterFile.read() + + if os.path.exists(outputPath): + os.remove(outputPath) + + for i in range(MAX_NUM + 1): + template += getter.replace("${num}", str(i)) + + template += "}\n" + + with open(outputPath, "w") as f: + f.write(template) diff --git a/wpiutil/src/generate/GenericNumber.java.in b/wpiutil/src/generate/GenericNumber.java.in new file mode 100644 index 0000000000..5a36582e4d --- /dev/null +++ b/wpiutil/src/generate/GenericNumber.java.in @@ -0,0 +1,34 @@ +/*----------------------------------------------------------------------------*/ +/* 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.numbers; + +import edu.wpi.first.wpiutil.math.Nat; +import edu.wpi.first.wpiutil.math.Num; + +/** + * A class representing the number ${num}. +*/ +public final class N${num} extends Num implements Nat { + private N${num}() { + } + + /** + * The integer this class represents. + * + * @return The literal number ${num}. + */ + @Override + public int getNum() { + return ${num}; + } + + /** + * The singleton instance of this class. + */ + public static final N${num} instance = new N${num}(); +} diff --git a/wpiutil/src/generate/Nat.java.in b/wpiutil/src/generate/Nat.java.in new file mode 100644 index 0000000000..5263c53a58 --- /dev/null +++ b/wpiutil/src/generate/Nat.java.in @@ -0,0 +1,45 @@ +/*----------------------------------------------------------------------------*/ +/* 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.N0; +import edu.wpi.first.wpiutil.math.numbers.N1; +import edu.wpi.first.wpiutil.math.numbers.N10; +import edu.wpi.first.wpiutil.math.numbers.N11; +import edu.wpi.first.wpiutil.math.numbers.N12; +import edu.wpi.first.wpiutil.math.numbers.N13; +import edu.wpi.first.wpiutil.math.numbers.N14; +import edu.wpi.first.wpiutil.math.numbers.N15; +import edu.wpi.first.wpiutil.math.numbers.N16; +import edu.wpi.first.wpiutil.math.numbers.N17; +import edu.wpi.first.wpiutil.math.numbers.N18; +import edu.wpi.first.wpiutil.math.numbers.N19; +import edu.wpi.first.wpiutil.math.numbers.N2; +import edu.wpi.first.wpiutil.math.numbers.N20; +import edu.wpi.first.wpiutil.math.numbers.N3; +import edu.wpi.first.wpiutil.math.numbers.N4; +import edu.wpi.first.wpiutil.math.numbers.N5; +import edu.wpi.first.wpiutil.math.numbers.N6; +import edu.wpi.first.wpiutil.math.numbers.N7; +import edu.wpi.first.wpiutil.math.numbers.N8; +import edu.wpi.first.wpiutil.math.numbers.N9; + +/** + * A natural number expressed as a java class. + * The counterpart to {@link Num} that should be used as a concrete value. + * + * @param The {@link Num} this represents. + */ +@SuppressWarnings({"MethodName", "unused", "PMD.TooManyMethods"}) +public interface Nat { + /** + * The number this interface represents. + * + * @return The number backing this value. + */ + int getNum(); diff --git a/wpiutil/src/generate/NatGetter.java.in b/wpiutil/src/generate/NatGetter.java.in new file mode 100644 index 0000000000..d268fab425 --- /dev/null +++ b/wpiutil/src/generate/NatGetter.java.in @@ -0,0 +1,3 @@ + static Nat N${num}() { + return N${num}.instance; + } diff --git a/wpiutil/src/main/java/edu/wpi/first/wpiutil/math/MatBuilder.java b/wpiutil/src/main/java/edu/wpi/first/wpiutil/math/MatBuilder.java new file mode 100644 index 0000000000..a5490a33d4 --- /dev/null +++ b/wpiutil/src/main/java/edu/wpi/first/wpiutil/math/MatBuilder.java @@ -0,0 +1,50 @@ +/*----------------------------------------------------------------------------*/ +/* 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 The number of rows of the desired matrix. + * @param The number of columns of the desired matrix. + */ +public class MatBuilder { + private final Nat m_rows; + private final Nat 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 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 rows, Nat cols) { + this.m_rows = Objects.requireNonNull(rows); + this.m_cols = Objects.requireNonNull(cols); + } +} diff --git a/wpiutil/src/main/java/edu/wpi/first/wpiutil/math/Matrix.java b/wpiutil/src/main/java/edu/wpi/first/wpiutil/math/Matrix.java new file mode 100644 index 0000000000..5780498b30 --- /dev/null +++ b/wpiutil/src/main/java/edu/wpi/first/wpiutil/math/Matrix.java @@ -0,0 +1,327 @@ +/*----------------------------------------------------------------------------*/ +/* 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. + * + *

This class is intended to be used alongside the state space library. + * + * @param The number of rows in this matrix. + * @param The number of columns in this matrix. + */ +@SuppressWarnings("PMD.TooManyMethods") +public class Matrix { + + 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 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. + * + *

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 The number of columns in the second matrix. + * @return The result of the matrix multiplication between this and the given matrix. + */ + public final Matrix times(Matrix 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 times(double value) { + return new Matrix<>(this.m_storage.scale(value)); + } + + /** + *

+ * Returns a matrix which is the result of an element by element multiplication of 'this' and 'b'. + * ci,j = ai,j*bi,j + *

+ * + * @param other A matrix. + * @return The element by element multiplication of 'this' and 'b'. + */ + public final Matrix elementTimes(Matrix 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 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 minus(Matrix 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 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 plus(Matrix 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 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 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 transpose() { + return new Matrix<>(this.m_storage.transpose()); + } + + + /** + * Returns a copy of this matrix. + * + * @return A copy of this matrix. + */ + public final Matrix 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 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.
+ *
+ * normF = Sqrt{ ∑i=1:m ∑j=1:n { aij2} } + * + * @return The matrix's Frobenius normal. + */ + public final double normF() { + return this.m_storage.normF(); + } + + /** + * Computes the induced p = 1 matrix norm.
+ *
+ * ||A||1= max(j=1 to n; sum(i=1 to m; |aij|)) + * + * @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': + * ci,j = ai,j ^ b. + * + * @param b Scalar + * @return The element by element power of 'this' and 'b'. + */ + @SuppressWarnings("ParameterName") + public final Matrix 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': + * ci,j = ai,j ^ b. + * + * @param b Scalar. + * @return The element by element power of 'this' and 'b'. + */ + @SuppressWarnings("ParameterName") + public final Matrix 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); + } +} diff --git a/wpiutil/src/main/java/edu/wpi/first/wpiutil/math/MatrixUtils.java b/wpiutil/src/main/java/edu/wpi/first/wpiutil/math/MatrixUtils.java new file mode 100644 index 0000000000..ac4deb9320 --- /dev/null +++ b/wpiutil/src/main/java/edu/wpi/first/wpiutil/math/MatrixUtils.java @@ -0,0 +1,83 @@ +/*----------------------------------------------------------------------------*/ +/* 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 The number of rows in the matrix as a generic. + * @param The number of columns in the matrix as a generic. + * @return An RxC matrix filled with zeros. + */ + @SuppressWarnings("LineLength") + public static Matrix zeros(Nat rows, Nat 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 The size of the desired vector as a generic. + * @return A vector of size N filled with zeros. + */ + public static Matrix zeros(Nat 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 The dimension of the desired matrix as a generic. + * @return The DxD identity matrix. + */ + public static Matrix eye(Nat 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 The number of rows of the desired matrix as a generic. + * @param The number of columns of the desired matrix as a generic. + * @return A builder to construct the matrix. + */ + public static MatBuilder mat(Nat rows, Nat 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 The dimension of the vector as a generic. + * @return A builder to construct the vector. + */ + public static VecBuilder vec(Nat dim) { + return new VecBuilder<>(dim); + } +} diff --git a/wpiutil/src/main/java/edu/wpi/first/wpiutil/math/Num.java b/wpiutil/src/main/java/edu/wpi/first/wpiutil/math/Num.java new file mode 100644 index 0000000000..c7385ea62c --- /dev/null +++ b/wpiutil/src/main/java/edu/wpi/first/wpiutil/math/Num.java @@ -0,0 +1,20 @@ +/*----------------------------------------------------------------------------*/ +/* 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(); +} diff --git a/wpiutil/src/main/java/edu/wpi/first/wpiutil/math/SimpleMatrixUtils.java b/wpiutil/src/main/java/edu/wpi/first/wpiutil/math/SimpleMatrixUtils.java new file mode 100644 index 0000000000..ea983d47ec --- /dev/null +++ b/wpiutil/src/main/java/edu/wpi/first/wpiutil/math/SimpleMatrixUtils.java @@ -0,0 +1,161 @@ +/*----------------------------------------------------------------------------*/ +/* 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 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 pair = _pade3(A); + return dispatchPade(pair.getFirst(), pair.getSecond(), n_squarings, solveProvider); + } else if (A_L1 < 2.539398330063230e-001) { + Pair pair = _pade5(A); + return dispatchPade(pair.getFirst(), pair.getSecond(), n_squarings, solveProvider); + } else if (A_L1 < 9.504178996162932e-001) { + Pair pair = _pade7(A); + return dispatchPade(pair.getFirst(), pair.getSecond(), n_squarings, solveProvider); + } else if (A_L1 < 2.097847961257068e+000) { + Pair 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 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 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 _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 _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 _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 _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 _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 { + 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; + } + } +} diff --git a/wpiutil/src/main/java/edu/wpi/first/wpiutil/math/VecBuilder.java b/wpiutil/src/main/java/edu/wpi/first/wpiutil/math/VecBuilder.java new file mode 100644 index 0000000000..deaaa41725 --- /dev/null +++ b/wpiutil/src/main/java/edu/wpi/first/wpiutil/math/VecBuilder.java @@ -0,0 +1,21 @@ +/*----------------------------------------------------------------------------*/ +/* 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 The dimension of the vector to be constructed. + */ +public class VecBuilder extends MatBuilder { + public VecBuilder(Nat rows) { + super(rows, Nat.N1()); + } +} diff --git a/wpiutil/src/test/java/edu/wpi/first/wpiutil/math/MatrixTest.java b/wpiutil/src/test/java/edu/wpi/first/wpiutil/math/MatrixTest.java new file mode 100644 index 0000000000..4d6697d3b1 --- /dev/null +++ b/wpiutil/src/test/java/edu/wpi/first/wpiutil/math/MatrixTest.java @@ -0,0 +1,210 @@ +/*----------------------------------------------------------------------------*/ +/* 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 org.ejml.data.SingularMatrixException; +import org.ejml.dense.row.MatrixFeatures_DDRM; +import org.ejml.simple.SimpleMatrix; +import org.junit.jupiter.api.Test; + +import edu.wpi.first.wpiutil.math.numbers.N1; +import edu.wpi.first.wpiutil.math.numbers.N2; +import edu.wpi.first.wpiutil.math.numbers.N3; +import edu.wpi.first.wpiutil.math.numbers.N4; + +import static org.junit.jupiter.api.Assertions.assertThrows; +import static org.junit.jupiter.api.Assertions.assertTrue; + +public class MatrixTest { + @Test + void testMatrixMultiplication() { + var mat1 = MatrixUtils.mat(Nat.N2(), Nat.N2()) + .fill(2.0, 1.0, + 0.0, 1.0); + var mat2 = MatrixUtils.mat(Nat.N2(), Nat.N2()) + .fill(3.0, 0.0, + 0.0, 2.5); + + Matrix result = mat1.times(mat2); + + assertTrue(MatrixFeatures_DDRM.isEquals( + MatrixUtils.mat(Nat.N2(), Nat.N2()) + .fill(6.0, 2.5, + 0.0, 2.5).getStorage().getDDRM(), + result.getStorage().getDDRM() + )); + + var mat3 = MatrixUtils.mat(Nat.N2(), Nat.N3()) + .fill(1.0, 3.0, 0.5, + 2.0, 4.3, 1.2); + var mat4 = MatrixUtils.mat(Nat.N3(), Nat.N4()) + .fill(3.0, 1.5, 2.0, 4.5, + 2.3, 1.0, 1.6, 3.1, + 5.2, 2.1, 2.0, 1.0); + + Matrix result2 = mat3.times(mat4); + + assertTrue(MatrixFeatures_DDRM.isIdentical( + MatrixUtils.mat(Nat.N2(), Nat.N4()) + .fill(12.5, 5.55, 7.8, 14.3, + 22.13, 9.82, 13.28, 23.53).getStorage().getDDRM(), + result2.getStorage().getDDRM(), + 1E-9 + )); + } + + @Test + void testMatrixVectorMultiplication() { + var mat = MatrixUtils.mat(Nat.N2(), Nat.N2()) + .fill(1.0, 1.0, + 0.0, 1.0); + + var vec = MatrixUtils.vec(Nat.N2()) + .fill(3.0, + 2.0); + + Matrix result = mat.times(vec); + assertTrue(MatrixFeatures_DDRM.isEquals( + MatrixUtils.vec(Nat.N2()) + .fill(5.0, + 2.0).getStorage().getDDRM(), + result.getStorage().getDDRM() + )); + } + + @Test + void testTranspose() { + Matrix vec = MatrixUtils.vec(Nat.N3()) + .fill(1.0, + 2.0, + 3.0); + + Matrix transpose = vec.transpose(); + + assertTrue(MatrixFeatures_DDRM.isEquals( + MatrixUtils.mat(Nat.N1(), Nat.N3()).fill(1.0, 2.0, 3.0).getStorage() + .getDDRM(), + transpose.getStorage().getDDRM() + )); + } + + @Test + void testInverse() { + var mat = MatrixUtils.mat(Nat.N3(), Nat.N3()) + .fill(1.0, 3.0, 2.0, + 5.0, 2.0, 1.5, + 0.0, 1.3, 2.5); + + var inv = mat.inv(); + + assertTrue(MatrixFeatures_DDRM.isIdentical( + MatrixUtils.eye(Nat.N3()).getStorage().getDDRM(), + mat.times(inv).getStorage().getDDRM(), + 1E-9 + )); + + assertTrue(MatrixFeatures_DDRM.isIdentical( + MatrixUtils.eye(Nat.N3()).getStorage().getDDRM(), + inv.times(mat).getStorage().getDDRM(), + 1E-9 + )); + } + + @Test + void testUninvertableMatrix() { + var singularMatrix = MatrixUtils.mat(Nat.N2(), Nat.N2()) + .fill(2.0, 1.0, + 2.0, 1.0); + + assertThrows(SingularMatrixException.class, singularMatrix::inv); + } + + @Test + void testMatrixScalarArithmetic() { + var mat = MatrixUtils.mat(Nat.N2(), Nat.N2()) + .fill(1.0, 2.0, + 3.0, 4.0); + + + assertTrue(MatrixFeatures_DDRM.isEquals( + MatrixUtils.mat(Nat.N2(), Nat.N2()) + .fill(3.0, 4.0, + 5.0, 6.0).getStorage().getDDRM(), + mat.plus(2.0).getStorage().getDDRM() + )); + + assertTrue(MatrixFeatures_DDRM.isEquals( + MatrixUtils.mat(Nat.N2(), Nat.N2()) + .fill(0.0, 1.0, + 2.0, 3.0).getStorage().getDDRM(), + mat.minus(1.0).getStorage().getDDRM() + )); + + assertTrue(MatrixFeatures_DDRM.isEquals( + MatrixUtils.mat(Nat.N2(), Nat.N2()) + .fill(2.0, 4.0, + 6.0, 8.0).getStorage().getDDRM(), + mat.times(2.0).getStorage().getDDRM() + )); + + assertTrue(MatrixFeatures_DDRM.isIdentical( + MatrixUtils.mat(Nat.N2(), Nat.N2()) + .fill(0.5, 1.0, + 1.5, 2.0).getStorage().getDDRM(), + mat.div(2.0).getStorage().getDDRM(), + 1E-3 + )); + } + + @Test + void testMatrixMatrixArithmetic() { + var mat1 = MatrixUtils.mat(Nat.N2(), Nat.N2()) + .fill(1.0, 2.0, + 3.0, 4.0); + + var mat2 = MatrixUtils.mat(Nat.N2(), Nat.N2()) + .fill(5.0, 6.0, + 7.0, 8.0); + + assertTrue(MatrixFeatures_DDRM.isEquals( + MatrixUtils.mat(Nat.N2(), Nat.N2()) + .fill(-4.0, -4.0, + -4.0, -4.0).getStorage().getDDRM(), + mat1.minus(mat2).getStorage().getDDRM() + )); + + assertTrue(MatrixFeatures_DDRM.isEquals( + MatrixUtils.mat(Nat.N2(), Nat.N2()) + .fill(6.0, 8.0, + 10.0, 12.0).getStorage().getDDRM(), + mat1.plus(mat2).getStorage().getDDRM() + )); + } + + @Test + void testMatrixExponential() { + SimpleMatrix matrix = MatrixUtils.eye(Nat.N2()).getStorage(); + var result = SimpleMatrixUtils.expm(matrix); + + assertTrue(MatrixFeatures_DDRM.isIdentical( + result.getDDRM(), + new SimpleMatrix(2, 2, true, new double[]{Math.E, 0, 0, Math.E}).getDDRM(), + 1E-9 + )); + + matrix = new SimpleMatrix(2, 2, true, new double[]{1, 2, 3, 4}); + result = SimpleMatrixUtils.expm(matrix.scale(0.01)); + + assertTrue(MatrixFeatures_DDRM.isIdentical( + result.getDDRM(), + new SimpleMatrix(2, 2, true, new double[]{1.01035625, 0.02050912, + 0.03076368, 1.04111993}).getDDRM(), + 1E-8 + )); + } +}