WPILibC++ 2025.1.1
Loading...
Searching...
No Matches
matd.h File Reference
#include <assert.h>
#include <stddef.h>
#include <string.h>

Go to the source code of this file.

Classes

struct  matd_t
 Defines a matrix structure for holding double-precision values with data in row-major order (i.e. More...
 
struct  matd_svd_t
 
struct  matd_plu_t
 
struct  matd_chol_t
 Creates a double matrix with the Cholesky lower triangular matrix of A. More...
 

Macros

#define MATD_ALLOC(name, nrows, ncols)
 
#define MATD_EPS   1e-8
 Defines a small value which can be used in place of zero for approximating calculations which are singular at zero values (i.e.
 
#define MATD_EL(m, row, col)
 A macro to reference a specific matd_t data element given it's zero-based row and column indexes.
 
#define MATD_SVD_NO_WARNINGS   1
 

Functions

matd_tmatd_create (int rows, int cols)
 Creates a double matrix with the given number of rows and columns (or a scalar in the case where rows=0 and/or cols=0).
 
matd_tmatd_create_data (int rows, int cols, const double *data)
 Creates a double matrix with the given number of rows and columns (or a scalar in the case where rows=0 and/or cols=0).
 
matd_tmatd_create_dataf (int rows, int cols, const float *data)
 Creates a double matrix with the given number of rows and columns (or a scalar in the case where rows=0 and/or cols=0).
 
matd_tmatd_identity (int dim)
 Creates a square identity matrix with the given number of rows (and therefore columns), or a scalar with value 1 in the case where dim=0.
 
matd_tmatd_create_scalar (double v)
 Creates a scalar with the supplied value 'v'.
 
double matd_get (const matd_t *m, unsigned int row, unsigned int col)
 Retrieves the cell value for matrix 'm' at the given zero-based row and column index.
 
void matd_put (matd_t *m, unsigned int row, unsigned int col, double value)
 Assigns the given value to the matrix cell at the given zero-based row and column index.
 
double matd_get_scalar (const matd_t *m)
 Retrieves the scalar value of the given element ('m' must be a scalar).
 
void matd_put_scalar (matd_t *m, double value)
 Assigns the given value to the supplied scalar element ('m' must be a scalar).
 
matd_tmatd_copy (const matd_t *m)
 Creates an exact copy of the supplied matrix 'm'.
 
matd_tmatd_select (const matd_t *a, unsigned int r0, int r1, unsigned int c0, int c1)
 Creates a copy of a subset of the supplied matrix 'a'.
 
void matd_print (const matd_t *m, const char *fmt)
 Prints the supplied matrix 'm' to standard output by applying the supplied printf format specifier 'fmt' for each individual element.
 
void matd_print_transpose (const matd_t *m, const char *fmt)
 Prints the transpose of the supplied matrix 'm' to standard output by applying the supplied printf format specifier 'fmt' for each individual element.
 
matd_tmatd_add (const matd_t *a, const matd_t *b)
 Adds the two supplied matrices together, cell-by-cell, and returns the results as a new matrix of the same dimensions.
 
void matd_add_inplace (matd_t *a, const matd_t *b)
 Adds the values of 'b' to matrix 'a', cell-by-cell, and overwrites the contents of 'a' with the results.
 
matd_tmatd_subtract (const matd_t *a, const matd_t *b)
 Subtracts matrix 'b' from matrix 'a', cell-by-cell, and returns the results as a new matrix of the same dimensions.
 
void matd_subtract_inplace (matd_t *a, const matd_t *b)
 Subtracts the values of 'b' from matrix 'a', cell-by-cell, and overwrites the contents of 'a' with the results.
 
matd_tmatd_scale (const matd_t *a, double s)
 Scales all cell values of matrix 'a' by the given scale factor 's' and returns the result as a new matrix of the same dimensions.
 
void matd_scale_inplace (matd_t *a, double s)
 Scales all cell values of matrix 'a' by the given scale factor 's' and overwrites the contents of 'a' with the results.
 
matd_tmatd_multiply (const matd_t *a, const matd_t *b)
 Multiplies the two supplied matrices together (matrix product), and returns the results as a new matrix.
 
matd_tmatd_transpose (const matd_t *a)
 Creates a matrix which is the transpose of the supplied matrix 'a'.
 
double matd_det (const matd_t *a)
 Calculates the determinant of the supplied matrix 'a'.
 
matd_tmatd_inverse (const matd_t *a)
 Attempts to compute an inverse of the supplied matrix 'a' and return it as a new matrix.
 
static void matd_set_data (matd_t *m, const double *data)
 
static int matd_is_scalar (const matd_t *a)
 Determines whether the supplied matrix 'a' is a scalar (positive return) or not (zero return, indicating a matrix of dimensions at least 1x1).
 
static int matd_is_vector (const matd_t *a)
 Determines whether the supplied matrix 'a' is a row or column vector (positive return) or not (zero return, indicating either 'a' is a scalar or a matrix with at least one dimension > 1).
 
static int matd_is_vector_len (const matd_t *a, int len)
 Determines whether the supplied matrix 'a' is a row or column vector with a dimension of 'len' (positive return) or not (zero return).
 
double matd_vec_mag (const matd_t *a)
 Calculates the magnitude of the supplied matrix 'a'.
 
double matd_vec_dist (const matd_t *a, const matd_t *b)
 Calculates the magnitude of the distance between the points represented by matrices 'a' and 'b'.
 
double matd_vec_dist_n (const matd_t *a, const matd_t *b, int n)
 Same as matd_vec_dist, but only uses the first 'n' terms to compute distance.
 
double matd_vec_dot_product (const matd_t *a, const matd_t *b)
 Calculates the dot product of two vectors.
 
matd_tmatd_vec_normalize (const matd_t *a)
 Calculates the normalization of the supplied vector 'a' (i.e.
 
matd_tmatd_crossproduct (const matd_t *a, const matd_t *b)
 Calculates the cross product of supplied matrices 'a' and 'b' (i.e.
 
double matd_err_inf (const matd_t *a, const matd_t *b)
 
matd_tmatd_op (const char *expr,...)
 Creates a new matrix by applying a series of matrix operations, as expressed in 'expr', to the supplied list of matrices.
 
void matd_destroy (matd_t *m)
 Frees the memory associated with matrix 'm', being the result of an earlier call to a matd_*() function, after which 'm' will no longer be usable.
 
matd_svd_t matd_svd (matd_t *A)
 Compute a complete SVD of a matrix.
 
matd_svd_t matd_svd_flags (matd_t *A, int flags)
 
matd_plu_tmatd_plu (const matd_t *a)
 
void matd_plu_destroy (matd_plu_t *mlu)
 
double matd_plu_det (const matd_plu_t *lu)
 
matd_tmatd_plu_p (const matd_plu_t *lu)
 
matd_tmatd_plu_l (const matd_plu_t *lu)
 
matd_tmatd_plu_u (const matd_plu_t *lu)
 
matd_tmatd_plu_solve (const matd_plu_t *mlu, const matd_t *b)
 
matd_tmatd_solve (matd_t *A, matd_t *b)
 
matd_chol_tmatd_chol (matd_t *A)
 
matd_tmatd_chol_solve (const matd_chol_t *chol, const matd_t *b)
 
void matd_chol_destroy (matd_chol_t *chol)
 
matd_tmatd_chol_inverse (matd_t *a)
 
void matd_ltransposetriangle_solve (matd_t *u, const double *b, double *x)
 
void matd_ltriangle_solve (matd_t *u, const double *b, double *x)
 
void matd_utriangle_solve (matd_t *u, const double *b, double *x)
 
double matd_max (matd_t *m)
 

Macro Definition Documentation

◆ MATD_ALLOC

#define MATD_ALLOC ( name,
nrows,
ncols )
Value:
double name ## _storage [nrows*ncols]; matd_t name = { .nrows = nrows, .ncols = ncols, .data = &name ## _storage };
Defines a matrix structure for holding double-precision values with data in row-major order (i....
Definition matd.h:46

◆ MATD_EL

#define MATD_EL ( m,
row,
col )
Value:
(m)->data[((row)*(m)->ncols + (col))]

A macro to reference a specific matd_t data element given it's zero-based row and column indexes.

Suitable for both retrieval and assignment.

◆ MATD_EPS

#define MATD_EPS   1e-8

Defines a small value which can be used in place of zero for approximating calculations which are singular at zero values (i.e.

inverting a matrix with a zero or near-zero determinant).

◆ MATD_SVD_NO_WARNINGS

#define MATD_SVD_NO_WARNINGS   1

Function Documentation

◆ matd_add()

matd_t * matd_add ( const matd_t * a,
const matd_t * b )

Adds the two supplied matrices together, cell-by-cell, and returns the results as a new matrix of the same dimensions.

The supplied matrices must have identical dimensions. It is the caller's responsibility to call matd_destroy() on the returned matrix.

◆ matd_add_inplace()

void matd_add_inplace ( matd_t * a,
const matd_t * b )

Adds the values of 'b' to matrix 'a', cell-by-cell, and overwrites the contents of 'a' with the results.

The supplied matrices must have identical dimensions.

◆ matd_chol()

matd_chol_t * matd_chol ( matd_t * A)

◆ matd_chol_destroy()

void matd_chol_destroy ( matd_chol_t * chol)

◆ matd_chol_inverse()

matd_t * matd_chol_inverse ( matd_t * a)

◆ matd_chol_solve()

matd_t * matd_chol_solve ( const matd_chol_t * chol,
const matd_t * b )

◆ matd_copy()

matd_t * matd_copy ( const matd_t * m)

Creates an exact copy of the supplied matrix 'm'.

It is the caller's responsibility to call matd_destroy() on the returned matrix.

◆ matd_create()

matd_t * matd_create ( int rows,
int cols )

Creates a double matrix with the given number of rows and columns (or a scalar in the case where rows=0 and/or cols=0).

All data elements will be initialized to zero. It is the caller's responsibility to call matd_destroy() on the returned matrix.

◆ matd_create_data()

matd_t * matd_create_data ( int rows,
int cols,
const double * data )

Creates a double matrix with the given number of rows and columns (or a scalar in the case where rows=0 and/or cols=0).

All data elements will be initialized using the supplied array of data, which must contain at least rows*cols elements, arranged in row-major order (i.e. index = row*ncols + col). It is the caller's responsibility to call matd_destroy() on the returned matrix.

◆ matd_create_dataf()

matd_t * matd_create_dataf ( int rows,
int cols,
const float * data )

Creates a double matrix with the given number of rows and columns (or a scalar in the case where rows=0 and/or cols=0).

All data elements will be initialized using the supplied array of float data, which must contain at least rows*cols elements, arranged in row-major order (i.e. index = row*ncols + col). It is the caller's responsibility to call matd_destroy() on the returned matrix.

◆ matd_create_scalar()

matd_t * matd_create_scalar ( double v)

Creates a scalar with the supplied value 'v'.

It is the caller's responsibility to call matd_destroy() on the returned matrix.

NOTE: Scalars are different than 1x1 matrices (implementation note: they are encoded as 0x0 matrices). For example: for matrices A*B, A and B must both have specific dimensions. However, if A is a scalar, there are no restrictions on the size of B.

◆ matd_crossproduct()

matd_t * matd_crossproduct ( const matd_t * a,
const matd_t * b )

Calculates the cross product of supplied matrices 'a' and 'b' (i.e.

a x b) and returns it as a new matrix. Both 'a' and 'b' must be vectors of dimension 3, but can be either row or column vectors. It is the caller's responsibility to call matd_destroy() on the returned matrix.

◆ matd_destroy()

void matd_destroy ( matd_t * m)

Frees the memory associated with matrix 'm', being the result of an earlier call to a matd_*() function, after which 'm' will no longer be usable.

◆ matd_det()

double matd_det ( const matd_t * a)

Calculates the determinant of the supplied matrix 'a'.

◆ matd_err_inf()

double matd_err_inf ( const matd_t * a,
const matd_t * b )

◆ matd_get()

double matd_get ( const matd_t * m,
unsigned int row,
unsigned int col )

Retrieves the cell value for matrix 'm' at the given zero-based row and column index.

Performs more thorough validation checking than MATD_EL().

◆ matd_get_scalar()

double matd_get_scalar ( const matd_t * m)

Retrieves the scalar value of the given element ('m' must be a scalar).

Performs more thorough validation checking than MATD_EL().

◆ matd_identity()

matd_t * matd_identity ( int dim)

Creates a square identity matrix with the given number of rows (and therefore columns), or a scalar with value 1 in the case where dim=0.

It is the caller's responsibility to call matd_destroy() on the returned matrix.

◆ matd_inverse()

matd_t * matd_inverse ( const matd_t * a)

Attempts to compute an inverse of the supplied matrix 'a' and return it as a new matrix.

This is strictly only possible if the determinant of 'a' is non-zero (matd_det(a) != 0).

If the determinant is zero, NULL is returned. It is otherwise the caller's responsibility to cope with the results caused by poorly conditioned matrices. (E.g.., if such a situation is likely to arise, compute the pseudo-inverse from the SVD.)

◆ matd_is_scalar()

static int matd_is_scalar ( const matd_t * a)
inlinestatic

Determines whether the supplied matrix 'a' is a scalar (positive return) or not (zero return, indicating a matrix of dimensions at least 1x1).

◆ matd_is_vector()

static int matd_is_vector ( const matd_t * a)
inlinestatic

Determines whether the supplied matrix 'a' is a row or column vector (positive return) or not (zero return, indicating either 'a' is a scalar or a matrix with at least one dimension > 1).

◆ matd_is_vector_len()

static int matd_is_vector_len ( const matd_t * a,
int len )
inlinestatic

Determines whether the supplied matrix 'a' is a row or column vector with a dimension of 'len' (positive return) or not (zero return).

◆ matd_ltransposetriangle_solve()

void matd_ltransposetriangle_solve ( matd_t * u,
const double * b,
double * x )

◆ matd_ltriangle_solve()

void matd_ltriangle_solve ( matd_t * u,
const double * b,
double * x )

◆ matd_max()

double matd_max ( matd_t * m)

◆ matd_multiply()

matd_t * matd_multiply ( const matd_t * a,
const matd_t * b )

Multiplies the two supplied matrices together (matrix product), and returns the results as a new matrix.

The supplied matrices must have dimensions such that columns(a) = rows(b). The returned matrix will have a row count of rows(a) and a column count of columns(b). It is the caller's responsibility to call matd_destroy() on the returned matrix.

◆ matd_op()

matd_t * matd_op ( const char * expr,
... )

Creates a new matrix by applying a series of matrix operations, as expressed in 'expr', to the supplied list of matrices.

Each matrix to be operated upon must be represented in the expression by a separate matrix placeholder, 'M', and there must be one matrix supplied as an argument for each matrix placeholder in the expression. All rules and caveats of the corresponding matrix operations apply to the operated-on matrices. It is the caller's responsibility to call matd_destroy() on the returned matrix.

Available operators (in order of increasing precedence): M+M add two matrices together M-M subtract one matrix from another M*M multiply two matrices together (matrix product) MM multiply two matrices together (matrix product) -M negate a matrix M^-1 take the inverse of a matrix M' take the transpose of a matrix

Expressions can be combined together and grouped by enclosing them in parenthesis, i.e.: -M(M+M+M)-(M*M)^-1

Scalar values can be generated on-the-fly, i.e.: M*2.2 scales M by 2.2 -2+M adds -2 to all elements of M

All whitespace in the expression is ignored.

◆ matd_plu()

matd_plu_t * matd_plu ( const matd_t * a)

◆ matd_plu_destroy()

void matd_plu_destroy ( matd_plu_t * mlu)

◆ matd_plu_det()

double matd_plu_det ( const matd_plu_t * lu)

◆ matd_plu_l()

matd_t * matd_plu_l ( const matd_plu_t * lu)

◆ matd_plu_p()

matd_t * matd_plu_p ( const matd_plu_t * lu)

◆ matd_plu_solve()

matd_t * matd_plu_solve ( const matd_plu_t * mlu,
const matd_t * b )

◆ matd_plu_u()

matd_t * matd_plu_u ( const matd_plu_t * lu)

◆ matd_print()

void matd_print ( const matd_t * m,
const char * fmt )

Prints the supplied matrix 'm' to standard output by applying the supplied printf format specifier 'fmt' for each individual element.

Each row will be printed on a separate newline.

◆ matd_print_transpose()

void matd_print_transpose ( const matd_t * m,
const char * fmt )

Prints the transpose of the supplied matrix 'm' to standard output by applying the supplied printf format specifier 'fmt' for each individual element.

Each row will be printed on a separate newline.

◆ matd_put()

void matd_put ( matd_t * m,
unsigned int row,
unsigned int col,
double value )

Assigns the given value to the matrix cell at the given zero-based row and column index.

Performs more thorough validation checking than MATD_EL().

◆ matd_put_scalar()

void matd_put_scalar ( matd_t * m,
double value )

Assigns the given value to the supplied scalar element ('m' must be a scalar).

Performs more thorough validation checking than MATD_EL().

◆ matd_scale()

matd_t * matd_scale ( const matd_t * a,
double s )

Scales all cell values of matrix 'a' by the given scale factor 's' and returns the result as a new matrix of the same dimensions.

It is the caller's responsibility to call matd_destroy() on the returned matrix.

◆ matd_scale_inplace()

void matd_scale_inplace ( matd_t * a,
double s )

Scales all cell values of matrix 'a' by the given scale factor 's' and overwrites the contents of 'a' with the results.

◆ matd_select()

matd_t * matd_select ( const matd_t * a,
unsigned int r0,
int r1,
unsigned int c0,
int c1 )

Creates a copy of a subset of the supplied matrix 'a'.

The subset will include rows 'r0' through 'r1', inclusive ('r1' >= 'r0'), and columns 'c0' through 'c1', inclusive ('c1' >= 'c0'). All parameters are zero-based (i.e. matd_select(a, 0, 0, 0, 0) will return only the first cell). Cannot be used on scalars or to extend beyond the number of rows/columns of 'a'. It is the caller's responsibility to call matd_destroy() on the returned matrix.

◆ matd_set_data()

static void matd_set_data ( matd_t * m,
const double * data )
inlinestatic

◆ matd_solve()

matd_t * matd_solve ( matd_t * A,
matd_t * b )

◆ matd_subtract()

matd_t * matd_subtract ( const matd_t * a,
const matd_t * b )

Subtracts matrix 'b' from matrix 'a', cell-by-cell, and returns the results as a new matrix of the same dimensions.

The supplied matrices must have identical dimensions. It is the caller's responsibility to call matd_destroy() on the returned matrix.

◆ matd_subtract_inplace()

void matd_subtract_inplace ( matd_t * a,
const matd_t * b )

Subtracts the values of 'b' from matrix 'a', cell-by-cell, and overwrites the contents of 'a' with the results.

The supplied matrices must have identical dimensions.

◆ matd_svd()

matd_svd_t matd_svd ( matd_t * A)

Compute a complete SVD of a matrix.

The SVD exists for all matrices. For a matrix MxN, we will have:

A = U*S*V'

where A is MxN, U is MxM (and is an orthonormal basis), S is MxN (and is diagonal up to machine precision), and V is NxN (and is an orthonormal basis).

The caller is responsible for destroying U, S, and V.

◆ matd_svd_flags()

matd_svd_t matd_svd_flags ( matd_t * A,
int flags )

◆ matd_transpose()

matd_t * matd_transpose ( const matd_t * a)

Creates a matrix which is the transpose of the supplied matrix 'a'.

It is the caller's responsibility to call matd_destroy() on the returned matrix.

◆ matd_utriangle_solve()

void matd_utriangle_solve ( matd_t * u,
const double * b,
double * x )

◆ matd_vec_dist()

double matd_vec_dist ( const matd_t * a,
const matd_t * b )

Calculates the magnitude of the distance between the points represented by matrices 'a' and 'b'.

Both 'a' and 'b' must be vectors and have the same dimension (although one may be a row vector and one may be a column vector).

◆ matd_vec_dist_n()

double matd_vec_dist_n ( const matd_t * a,
const matd_t * b,
int n )

Same as matd_vec_dist, but only uses the first 'n' terms to compute distance.

◆ matd_vec_dot_product()

double matd_vec_dot_product ( const matd_t * a,
const matd_t * b )

Calculates the dot product of two vectors.

Both 'a' and 'b' must be vectors and have the same dimension (although one may be a row vector and one may be a column vector).

◆ matd_vec_mag()

double matd_vec_mag ( const matd_t * a)

Calculates the magnitude of the supplied matrix 'a'.

◆ matd_vec_normalize()

matd_t * matd_vec_normalize ( const matd_t * a)

Calculates the normalization of the supplied vector 'a' (i.e.

a unit vector of the same dimension and orientation as 'a' with a magnitude of 1) and returns it as a new vector. 'a' must be a vector of any dimension and must have a non-zero magnitude. It is the caller's responsibility to call matd_destroy() on the returned matrix.