Source code

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.math.numbers.N1;
008import java.util.Objects;
009import org.ejml.MatrixDimensionException;
010import org.ejml.data.DMatrixRMaj;
011import org.ejml.dense.row.CommonOps_DDRM;
012import org.ejml.dense.row.MatrixFeatures_DDRM;
013import org.ejml.dense.row.NormOps_DDRM;
014import org.ejml.dense.row.factory.DecompositionFactory_DDRM;
015import org.ejml.interfaces.decomposition.CholeskyDecomposition_F64;
016import org.ejml.simple.SimpleMatrix;
017
018/**
019 * A shape-safe wrapper over Efficient Java Matrix Library (EJML) matrices.
020 *
021 * <p>This class is intended to be used alongside the state space library.
022 *
023 * @param <R> The number of rows in this matrix.
024 * @param <C> The number of columns in this matrix.
025 */
026public class Matrix<R extends Num, C extends Num> {
027  protected final SimpleMatrix m_storage;
028
029  /**
030   * Constructs an empty zero matrix of the given dimensions.
031   *
032   * @param rows The number of rows of the matrix.
033   * @param columns The number of columns of the matrix.
034   */
035  public Matrix(Nat<R> rows, Nat<C> columns) {
036    this.m_storage =
037        new SimpleMatrix(
038            Objects.requireNonNull(rows).getNum(), Objects.requireNonNull(columns).getNum());
039  }
040
041  /**
042   * Constructs a new {@link Matrix} with the given storage. Caller should make sure that the
043   * provided generic bounds match the shape of the provided {@link Matrix}.
044   *
045   * @param rows The number of rows of the matrix.
046   * @param columns The number of columns of the matrix.
047   * @param storage The double array to back this value.
048   */
049  public Matrix(Nat<R> rows, Nat<C> columns, double[] storage) {
050    this.m_storage = new SimpleMatrix(rows.getNum(), columns.getNum(), true, storage);
051  }
052
053  /**
054   * Constructs a new {@link Matrix} with the given storage. Caller should make sure that the
055   * provided generic bounds match the shape of the provided {@link Matrix}.
056   *
057   * <p>NOTE:It is not recommend to use this constructor unless the {@link SimpleMatrix} API is
058   * absolutely necessary due to the desired function not being accessible through the {@link
059   * Matrix} wrapper.
060   *
061   * @param storage The {@link SimpleMatrix} to back this value.
062   */
063  public Matrix(SimpleMatrix storage) {
064    this.m_storage = Objects.requireNonNull(storage);
065  }
066
067  /**
068   * Constructs a new matrix with the storage of the supplied matrix.
069   *
070   * @param other The {@link Matrix} to copy the storage of.
071   */
072  public Matrix(Matrix<R, C> other) {
073    this.m_storage = Objects.requireNonNull(other).getStorage().copy();
074  }
075
076  /**
077   * Gets the underlying {@link SimpleMatrix} that this {@link Matrix} wraps.
078   *
079   * <p>NOTE:The use of this method is heavily discouraged as this removes any guarantee of type
080   * safety. This should only be called if the {@link SimpleMatrix} API is absolutely necessary due
081   * to the desired function not being accessible through the {@link Matrix} wrapper.
082   *
083   * @return The underlying {@link SimpleMatrix} storage.
084   */
085  public SimpleMatrix getStorage() {
086    return m_storage;
087  }
088
089  /**
090   * Gets the number of columns in this matrix.
091   *
092   * @return The number of columns, according to the internal storage.
093   */
094  public final int getNumCols() {
095    return this.m_storage.getNumCols();
096  }
097
098  /**
099   * Gets the number of rows in this matrix.
100   *
101   * @return The number of rows, according to the internal storage.
102   */
103  public final int getNumRows() {
104    return this.m_storage.getNumRows();
105  }
106
107  /**
108   * Get an element of this matrix.
109   *
110   * @param row The row of the element.
111   * @param col The column of the element.
112   * @return The element in this matrix at row,col.
113   */
114  public final double get(int row, int col) {
115    return this.m_storage.get(row, col);
116  }
117
118  /**
119   * Sets the value at the given indices.
120   *
121   * @param row The row of the element.
122   * @param col The column of the element.
123   * @param value The value to insert at the given location.
124   */
125  public final void set(int row, int col, double value) {
126    this.m_storage.set(row, col, value);
127  }
128
129  /**
130   * Sets a row to a given row vector.
131   *
132   * @param row The row to set.
133   * @param val The row vector to set the given row to.
134   */
135  public final void setRow(int row, Matrix<N1, C> val) {
136    this.m_storage.setRow(row, 0, Objects.requireNonNull(val).m_storage.getDDRM().getData());
137  }
138
139  /**
140   * Sets a column to a given column vector.
141   *
142   * @param column The column to set.
143   * @param val The column vector to set the given row to.
144   */
145  public final void setColumn(int column, Matrix<R, N1> val) {
146    this.m_storage.setColumn(column, 0, Objects.requireNonNull(val).m_storage.getDDRM().getData());
147  }
148
149  /**
150   * Sets all the elements in "this" matrix equal to the specified value.
151   *
152   * @param value The value each element is set to.
153   */
154  public void fill(double value) {
155    this.m_storage.fill(value);
156  }
157
158  /**
159   * Returns the diagonal elements inside a vector or square matrix.
160   *
161   * <p>If "this" {@link Matrix} is a vector then a square matrix is returned. If a "this" {@link
162   * Matrix} is a matrix then a vector of diagonal elements is returned.
163   *
164   * @return The diagonal elements inside a vector or a square matrix.
165   */
166  public final Matrix<R, C> diag() {
167    return new Matrix<>(this.m_storage.diag());
168  }
169
170  /**
171   * Returns the largest element of this matrix.
172   *
173   * @return The largest element of this matrix.
174   */
175  public final double max() {
176    return CommonOps_DDRM.elementMax(this.m_storage.getDDRM());
177  }
178
179  /**
180   * Returns the absolute value of the element in this matrix with the largest absolute value.
181   *
182   * @return The absolute value of the element with the largest absolute value.
183   */
184  public final double maxAbs() {
185    return CommonOps_DDRM.elementMaxAbs(this.m_storage.getDDRM());
186  }
187
188  /**
189   * Returns the smallest element of this matrix.
190   *
191   * @return The smallest element of this matrix.
192   */
193  public final double minInternal() {
194    return CommonOps_DDRM.elementMin(this.m_storage.getDDRM());
195  }
196
197  /**
198   * Calculates the mean of the elements in this matrix.
199   *
200   * @return The mean value of this matrix.
201   */
202  public final double mean() {
203    return this.elementSum() / (double) this.m_storage.getNumElements();
204  }
205
206  /**
207   * Multiplies this matrix with another that has C rows.
208   *
209   * <p>As matrix multiplication is only defined if the number of columns in the first matrix
210   * matches the number of rows in the second, this operation will fail to compile under any other
211   * circumstances.
212   *
213   * @param other The other matrix to multiply by.
214   * @param <C2> The number of columns in the second matrix.
215   * @return The result of the matrix multiplication between "this" and the given matrix.
216   */
217  public final <C2 extends Num> Matrix<R, C2> times(Matrix<C, C2> other) {
218    return new Matrix<>(this.m_storage.mult(Objects.requireNonNull(other).m_storage));
219  }
220
221  /**
222   * Multiplies all the elements of this matrix by the given scalar.
223   *
224   * @param value The scalar value to multiply by.
225   * @return A new matrix with all the elements multiplied by the given value.
226   */
227  public Matrix<R, C> times(double value) {
228    return new Matrix<>(this.m_storage.scale(value));
229  }
230
231  /**
232   * Returns a matrix which is the result of an element by element multiplication of "this" and
233   * other.
234   *
235   * <p>c<sub>i,j</sub> = a<sub>i,j</sub>*other<sub>i,j</sub>
236   *
237   * @param other The other {@link Matrix} to perform element multiplication on.
238   * @return The element by element multiplication of "this" and other.
239   */
240  public final Matrix<R, C> elementTimes(Matrix<R, C> other) {
241    return new Matrix<>(this.m_storage.elementMult(Objects.requireNonNull(other).m_storage));
242  }
243
244  /**
245   * Subtracts the given value from all the elements of this matrix.
246   *
247   * @param value The value to subtract.
248   * @return The resultant matrix.
249   */
250  public final Matrix<R, C> minus(double value) {
251    return new Matrix<>(this.m_storage.minus(value));
252  }
253
254  /**
255   * Subtracts the given matrix from this matrix.
256   *
257   * @param value The matrix to subtract.
258   * @return The resultant matrix.
259   */
260  public final Matrix<R, C> minus(Matrix<R, C> value) {
261    return new Matrix<>(this.m_storage.minus(Objects.requireNonNull(value).m_storage));
262  }
263
264  /**
265   * Adds the given value to all the elements of this matrix.
266   *
267   * @param value The value to add.
268   * @return The resultant matrix.
269   */
270  public final Matrix<R, C> plus(double value) {
271    return new Matrix<>(this.m_storage.plus(value));
272  }
273
274  /**
275   * Adds the given matrix to this matrix.
276   *
277   * @param value The matrix to add.
278   * @return The resultant matrix.
279   */
280  public final Matrix<R, C> plus(Matrix<R, C> value) {
281    return new Matrix<>(this.m_storage.plus(Objects.requireNonNull(value).m_storage));
282  }
283
284  /**
285   * Divides all elements of this matrix by the given value.
286   *
287   * @param value The value to divide by.
288   * @return The resultant matrix.
289   */
290  public Matrix<R, C> div(int value) {
291    return new Matrix<>(this.m_storage.divide(value));
292  }
293
294  /**
295   * Divides all elements of this matrix by the given value.
296   *
297   * @param value The value to divide by.
298   * @return The resultant matrix.
299   */
300  public Matrix<R, C> div(double value) {
301    return new Matrix<>(this.m_storage.divide(value));
302  }
303
304  /**
305   * Calculates the transpose, Mᵀ of this matrix.
306   *
307   * @return The transpose matrix.
308   */
309  public final Matrix<C, R> transpose() {
310    return new Matrix<>(this.m_storage.transpose());
311  }
312
313  /**
314   * Returns a copy of this matrix.
315   *
316   * @return A copy of this matrix.
317   */
318  public final Matrix<R, C> copy() {
319    return new Matrix<>(this.m_storage.copy());
320  }
321
322  /**
323   * Returns the inverse matrix of "this" matrix.
324   *
325   * @return The inverse of "this" matrix.
326   * @throws org.ejml.data.SingularMatrixException If "this" matrix is non-invertable.
327   */
328  public final Matrix<R, C> inv() {
329    return new Matrix<>(this.m_storage.invert());
330  }
331
332  /**
333   * Returns the solution x to the equation Ax = b, where A is "this" matrix.
334   *
335   * <p>The matrix equation could also be written as x = A<sup>-1</sup>b. Where the pseudo inverse
336   * is used if A is not square.
337   *
338   * <p>Note that this method does not support solving using a QR decomposition with full-pivoting,
339   * as only column-pivoting is supported. For full-pivoting, use {@link
340   * #solveFullPivHouseholderQr}.
341   *
342   * @param <C2> Columns in b.
343   * @param b The right-hand side of the equation to solve.
344   * @return The solution to the linear system.
345   */
346  public final <C2 extends Num> Matrix<C, C2> solve(Matrix<R, C2> b) {
347    return new Matrix<>(this.m_storage.solve(Objects.requireNonNull(b).m_storage));
348  }
349
350  /**
351   * Solves the least-squares problem Ax=B using a QR decomposition with full pivoting, where this
352   * matrix is A.
353   *
354   * @param <R2> Number of rows in B.
355   * @param <C2> Number of columns in B.
356   * @param other The B matrix.
357   * @return The solution matrix.
358   */
359  public final <R2 extends Num, C2 extends Num> Matrix<C, C2> solveFullPivHouseholderQr(
360      Matrix<R2, C2> other) {
361    Matrix<C, C2> solution = new Matrix<>(new SimpleMatrix(this.getNumCols(), other.getNumCols()));
362    WPIMathJNI.solveFullPivHouseholderQr(
363        this.getData(),
364        this.getNumRows(),
365        this.getNumCols(),
366        other.getData(),
367        other.getNumRows(),
368        other.getNumCols(),
369        solution.getData());
370    return solution;
371  }
372
373  /**
374   * Computes the matrix exponential using Eigen's solver. This method only works for square
375   * matrices, and will otherwise throw an {@link MatrixDimensionException}.
376   *
377   * @return The exponential of A.
378   */
379  public final Matrix<R, C> exp() {
380    if (this.getNumRows() != this.getNumCols()) {
381      throw new MatrixDimensionException(
382          "Non-square matrices cannot be exponentiated! "
383              + "This matrix is "
384              + this.getNumRows()
385              + " x "
386              + this.getNumCols());
387    }
388    Matrix<R, C> toReturn = new Matrix<>(new SimpleMatrix(this.getNumRows(), this.getNumCols()));
389    WPIMathJNI.exp(
390        this.m_storage.getDDRM().getData(),
391        this.getNumRows(),
392        toReturn.m_storage.getDDRM().getData());
393    return toReturn;
394  }
395
396  /**
397   * Computes the matrix power using Eigen's solver. This method only works for square matrices, and
398   * will otherwise throw an {@link MatrixDimensionException}.
399   *
400   * @param exponent The exponent.
401   * @return The exponential of A.
402   */
403  public final Matrix<R, C> pow(double exponent) {
404    if (this.getNumRows() != this.getNumCols()) {
405      throw new MatrixDimensionException(
406          "Non-square matrices cannot be raised to a power! "
407              + "This matrix is "
408              + this.getNumRows()
409              + " x "
410              + this.getNumCols());
411    }
412    Matrix<R, C> toReturn = new Matrix<>(new SimpleMatrix(this.getNumRows(), this.getNumCols()));
413    WPIMathJNI.pow(
414        this.m_storage.getDDRM().getData(),
415        this.getNumRows(),
416        exponent,
417        toReturn.m_storage.getDDRM().getData());
418    return toReturn;
419  }
420
421  /**
422   * Returns the determinant of this matrix.
423   *
424   * @return The determinant of this matrix.
425   */
426  public final double det() {
427    return this.m_storage.determinant();
428  }
429
430  /**
431   * Computes the Frobenius normal of the matrix.
432   *
433   * <p>normF = Sqrt{ &sum;<sub>i=1:m</sub> &sum;<sub>j=1:n</sub> { a<sub>ij</sub><sup>2</sup>} }
434   *
435   * @return The matrix's Frobenius normal.
436   */
437  public final double normF() {
438    return this.m_storage.normF();
439  }
440
441  /**
442   * Computes the induced p = 1 matrix norm.
443   *
444   * <p>||A||<sub>1</sub>= max(j=1 to n; sum(i=1 to m; |a<sub>ij</sub>|))
445   *
446   * @return The norm.
447   */
448  public final double normIndP1() {
449    return NormOps_DDRM.inducedP1(this.m_storage.getDDRM());
450  }
451
452  /**
453   * Computes the sum of all the elements in the matrix.
454   *
455   * @return Sum of all the elements.
456   */
457  public final double elementSum() {
458    return this.m_storage.elementSum();
459  }
460
461  /**
462   * Computes the trace of the matrix.
463   *
464   * @return The trace of the matrix.
465   */
466  public final double trace() {
467    return this.m_storage.trace();
468  }
469
470  /**
471   * Returns a matrix which is the result of an element by element power of "this" and b.
472   *
473   * <p>c<sub>i,j</sub> = a<sub>i,j</sub> ^ b
474   *
475   * @param b Scalar.
476   * @return The element by element power of "this" and b.
477   */
478  public final Matrix<R, C> elementPower(double b) {
479    return new Matrix<>(this.m_storage.elementPower(b));
480  }
481
482  /**
483   * Returns a matrix which is the result of an element by element power of "this" and b.
484   *
485   * <p>c<sub>i,j</sub> = a<sub>i,j</sub> ^ b
486   *
487   * @param b Scalar.
488   * @return The element by element power of "this" and b.
489   */
490  public final Matrix<R, C> elementPower(int b) {
491    return new Matrix<>(this.m_storage.elementPower(b));
492  }
493
494  /**
495   * Extracts a given row into a row vector with new underlying storage.
496   *
497   * @param row The row to extract a vector from.
498   * @return A row vector from the given row.
499   */
500  public final Matrix<N1, C> extractRowVector(int row) {
501    return new Matrix<>(this.m_storage.extractVector(true, row));
502  }
503
504  /**
505   * Extracts a given column into a column vector with new underlying storage.
506   *
507   * @param column The column to extract a vector from.
508   * @return A column vector from the given column.
509   */
510  public final Matrix<R, N1> extractColumnVector(int column) {
511    return new Matrix<>(this.m_storage.extractVector(false, column));
512  }
513
514  /**
515   * Extracts a matrix of a given size and start position with new underlying storage.
516   *
517   * @param <R2> Number of rows to extract.
518   * @param <C2> Number of columns to extract.
519   * @param height The number of rows of the extracted matrix.
520   * @param width The number of columns of the extracted matrix.
521   * @param startingRow The starting row of the extracted matrix.
522   * @param startingCol The starting column of the extracted matrix.
523   * @return The extracted matrix.
524   */
525  public final <R2 extends Num, C2 extends Num> Matrix<R2, C2> block(
526      Nat<R2> height, Nat<C2> width, int startingRow, int startingCol) {
527    return new Matrix<>(
528        this.m_storage.extractMatrix(
529            startingRow,
530            startingRow + Objects.requireNonNull(height).getNum(),
531            startingCol,
532            startingCol + Objects.requireNonNull(width).getNum()));
533  }
534
535  /**
536   * Extracts a matrix of a given size and start position with new underlying storage.
537   *
538   * @param <R2> Number of rows to extract.
539   * @param <C2> Number of columns to extract.
540   * @param height The number of rows of the extracted matrix.
541   * @param width The number of columns of the extracted matrix.
542   * @param startingRow The starting row of the extracted matrix.
543   * @param startingCol The starting column of the extracted matrix.
544   * @return The extracted matrix.
545   */
546  public final <R2 extends Num, C2 extends Num> Matrix<R2, C2> block(
547      int height, int width, int startingRow, int startingCol) {
548    return new Matrix<>(
549        this.m_storage.extractMatrix(
550            startingRow, startingRow + height, startingCol, startingCol + width));
551  }
552
553  /**
554   * Assign a matrix of a given size and start position.
555   *
556   * @param <R2> Rows in block assignment.
557   * @param <C2> Columns in block assignment.
558   * @param startingRow The row to start at.
559   * @param startingCol The column to start at.
560   * @param other The matrix to assign the block to.
561   */
562  public <R2 extends Num, C2 extends Num> void assignBlock(
563      int startingRow, int startingCol, Matrix<R2, C2> other) {
564    this.m_storage.insertIntoThis(
565        startingRow, startingCol, Objects.requireNonNull(other).m_storage);
566  }
567
568  /**
569   * Extracts a submatrix from the supplied matrix and inserts it in a submatrix in "this". The
570   * shape of "this" is used to determine the size of the matrix extracted.
571   *
572   * @param <R2> Number of rows to extract.
573   * @param <C2> Number of columns to extract.
574   * @param startingRow The starting row in the supplied matrix to extract the submatrix.
575   * @param startingCol The starting column in the supplied matrix to extract the submatrix.
576   * @param other The matrix to extract the submatrix from.
577   */
578  public <R2 extends Num, C2 extends Num> void extractFrom(
579      int startingRow, int startingCol, Matrix<R2, C2> other) {
580    CommonOps_DDRM.extract(
581        other.m_storage.getDDRM(), startingRow, startingCol, this.m_storage.getDDRM());
582  }
583
584  /**
585   * Decompose "this" matrix using Cholesky Decomposition. If the "this" matrix is zeros, it will
586   * return the zero matrix.
587   *
588   * @param lowerTriangular Whether we want to decompose to the lower triangular Cholesky matrix.
589   * @return The decomposed matrix.
590   * @throws RuntimeException if the matrix could not be decomposed(i.e. is not positive
591   *     semidefinite).
592   */
593  public Matrix<R, C> lltDecompose(boolean lowerTriangular) {
594    SimpleMatrix temp = m_storage.copy();
595
596    CholeskyDecomposition_F64<DMatrixRMaj> chol =
597        DecompositionFactory_DDRM.chol(temp.getNumRows(), lowerTriangular);
598    if (!chol.decompose(temp.getMatrix())) {
599      // check that the input is not all zeros -- if they are, we special case and return all
600      // zeros.
601      var matData = temp.getDDRM().data;
602      var isZeros = true;
603      for (double matDatum : matData) {
604        isZeros &= Math.abs(matDatum) < 1e-6;
605      }
606      if (isZeros) {
607        return new Matrix<>(new SimpleMatrix(temp.getNumRows(), temp.getNumCols()));
608      }
609
610      throw new RuntimeException("Cholesky decomposition failed! Input matrix:\n" + m_storage);
611    }
612
613    return new Matrix<>(SimpleMatrix.wrap(chol.getT(null)));
614  }
615
616  /**
617   * Returns the row major data of this matrix as a double array.
618   *
619   * @return The row major data of this matrix as a double array.
620   */
621  public double[] getData() {
622    return m_storage.getDDRM().getData();
623  }
624
625  /**
626   * Creates the identity matrix of the given dimension.
627   *
628   * @param dim The dimension of the desired matrix as a {@link Nat}.
629   * @param <D> The dimension of the desired matrix as a generic.
630   * @return The DxD identity matrix.
631   */
632  public static <D extends Num> Matrix<D, D> eye(Nat<D> dim) {
633    return new Matrix<>(SimpleMatrix.identity(Objects.requireNonNull(dim).getNum()));
634  }
635
636  /**
637   * Creates the identity matrix of the given dimension.
638   *
639   * @param dim The dimension of the desired matrix as a {@link Num}.
640   * @param <D> The dimension of the desired matrix as a generic.
641   * @return The DxD identity matrix.
642   */
643  public static <D extends Num> Matrix<D, D> eye(D dim) {
644    return new Matrix<>(SimpleMatrix.identity(Objects.requireNonNull(dim).getNum()));
645  }
646
647  /**
648   * Entrypoint to the {@link MatBuilder} class for creation of custom matrices with the given
649   * dimensions and contents.
650   *
651   * @param rows The number of rows of the desired matrix.
652   * @param cols The number of columns of the desired matrix.
653   * @param <R> The number of rows of the desired matrix as a generic.
654   * @param <C> The number of columns of the desired matrix as a generic.
655   * @return A builder to construct the matrix.
656   * @deprecated Use {@link MatBuilder#fill} instead.
657   */
658  @Deprecated(since = "2024", forRemoval = true)
659  @SuppressWarnings("removal")
660  public static <R extends Num, C extends Num> MatBuilder<R, C> mat(Nat<R> rows, Nat<C> cols) {
661    return new MatBuilder<>(Objects.requireNonNull(rows), Objects.requireNonNull(cols));
662  }
663
664  /**
665   * Reassigns dimensions of a {@link Matrix} to allow for operations with other matrices that have
666   * wildcard dimensions.
667   *
668   * @param <R1> Row dimension to assign.
669   * @param <C1> Column dimension to assign.
670   * @param mat The {@link Matrix} to remove the dimensions from.
671   * @return The matrix with reassigned dimensions.
672   */
673  public static <R1 extends Num, C1 extends Num> Matrix<R1, C1> changeBoundsUnchecked(
674      Matrix<?, ?> mat) {
675    return new Matrix<>(mat.m_storage);
676  }
677
678  /**
679   * Checks if another {@link Matrix} is identical to "this" one within a specified tolerance.
680   *
681   * <p>This will check if each element is in tolerance of the corresponding element from the other
682   * {@link Matrix} or if the elements have the same symbolic meaning. For two elements to have the
683   * same symbolic meaning they both must be either Double.NaN, Double.POSITIVE_INFINITY, or
684   * Double.NEGATIVE_INFINITY.
685   *
686   * <p>NOTE:It is recommended to use {@link Matrix#isEqual(Matrix, double)} over this method when
687   * checking if two matrices are equal as {@link Matrix#isEqual(Matrix, double)} will return false
688   * if an element is uncountable. This method should only be used when uncountable elements need to
689   * be compared.
690   *
691   * @param other The {@link Matrix} to check against this one.
692   * @param tolerance The tolerance to check equality with.
693   * @return true if this matrix is identical to the one supplied.
694   */
695  public boolean isIdentical(Matrix<?, ?> other, double tolerance) {
696    return MatrixFeatures_DDRM.isIdentical(
697        this.m_storage.getDDRM(), other.m_storage.getDDRM(), tolerance);
698  }
699
700  /**
701   * Checks if another {@link Matrix} is equal to "this" within a specified tolerance.
702   *
703   * <p>This will check if each element is in tolerance of the corresponding element from the other
704   * {@link Matrix}.
705   *
706   * <p>tol &ge; |a<sub>ij</sub> - b<sub>ij</sub>|
707   *
708   * @param other The {@link Matrix} to check against this one.
709   * @param tolerance The tolerance to check equality with.
710   * @return true if this matrix is equal to the one supplied.
711   */
712  public boolean isEqual(Matrix<?, ?> other, double tolerance) {
713    return MatrixFeatures_DDRM.isEquals(
714        this.m_storage.getDDRM(), other.m_storage.getDDRM(), tolerance);
715  }
716
717  /**
718   * Performs an inplace Cholesky rank update (or downdate).
719   *
720   * <p>If this matrix contains L where A = LLᵀ before the update, it will contain L where LLᵀ = A +
721   * σvvᵀ after the update.
722   *
723   * @param v Vector to use for the update.
724   * @param sigma Sigma to use for the update.
725   * @param lowerTriangular Whether this matrix is lower triangular.
726   */
727  public void rankUpdate(Matrix<R, N1> v, double sigma, boolean lowerTriangular) {
728    WPIMathJNI.rankUpdate(this.getData(), this.getNumRows(), v.getData(), sigma, lowerTriangular);
729  }
730
731  @Override
732  public String toString() {
733    return m_storage.toString();
734  }
735
736  /**
737   * Checks if an object is equal to this {@link Matrix}.
738   *
739   * <p>a<sub>ij</sub> == b<sub>ij</sub>
740   *
741   * @param other The Object to check against this {@link Matrix}.
742   * @return true if the object supplied is a {@link Matrix} and is equal to this matrix.
743   */
744  @Override
745  public boolean equals(Object other) {
746    if (this == other) {
747      return true;
748    }
749    if (!(other instanceof Matrix)) {
750      return false;
751    }
752
753    Matrix<?, ?> matrix = (Matrix<?, ?>) other;
754    if (MatrixFeatures_DDRM.hasUncountable(matrix.m_storage.getDDRM())) {
755      return false;
756    }
757    return MatrixFeatures_DDRM.isEquals(this.m_storage.getDDRM(), matrix.m_storage.getDDRM());
758  }
759
760  @Override
761  public int hashCode() {
762    return Objects.hash(m_storage);
763  }
764}