Austin Schuh | 3333ec7 | 2022-12-29 16:21:06 -0800 | [diff] [blame^] | 1 | /* Copyright (C) 2013-2016, The Regents of The University of Michigan. |
| 2 | All rights reserved. |
| 3 | This software was developed in the APRIL Robotics Lab under the |
| 4 | direction of Edwin Olson, ebolson@umich.edu. This software may be |
| 5 | available under alternative licensing terms; contact the address above. |
| 6 | Redistribution and use in source and binary forms, with or without |
| 7 | modification, are permitted provided that the following conditions are met: |
| 8 | 1. Redistributions of source code must retain the above copyright notice, this |
| 9 | list of conditions and the following disclaimer. |
| 10 | 2. Redistributions in binary form must reproduce the above copyright notice, |
| 11 | this list of conditions and the following disclaimer in the documentation |
| 12 | and/or other materials provided with the distribution. |
| 13 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND |
| 14 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED |
| 15 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE |
| 16 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR |
| 17 | ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES |
| 18 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; |
| 19 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND |
| 20 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT |
| 21 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS |
| 22 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. |
| 23 | The views and conclusions contained in the software and documentation are those |
| 24 | of the authors and should not be interpreted as representing official policies, |
| 25 | either expressed or implied, of the Regents of The University of Michigan. |
| 26 | */ |
| 27 | |
| 28 | #pragma once |
| 29 | |
| 30 | #include <assert.h> |
| 31 | #include <stddef.h> |
| 32 | #include <string.h> |
| 33 | |
| 34 | #ifdef __cplusplus |
| 35 | extern "C" { |
| 36 | #endif |
| 37 | |
| 38 | /** |
| 39 | * Defines a matrix structure for holding double-precision values with |
| 40 | * data in row-major order (i.e. index = row*ncols + col). |
| 41 | * |
| 42 | * nrows and ncols are 1-based counts with the exception that a scalar (non-matrix) |
| 43 | * is represented with nrows=0 and/or ncols=0. |
| 44 | */ |
| 45 | typedef struct |
| 46 | { |
| 47 | unsigned int nrows, ncols; |
| 48 | double data[]; |
| 49 | // double *data; |
| 50 | } matd_t; |
| 51 | |
| 52 | #define MATD_ALLOC(name, nrows, ncols) double name ## _storage [nrows*ncols]; matd_t name = { .nrows = nrows, .ncols = ncols, .data = &name ## _storage }; |
| 53 | |
| 54 | /** |
| 55 | * Defines a small value which can be used in place of zero for approximating |
| 56 | * calculations which are singular at zero values (i.e. inverting a matrix with |
| 57 | * a zero or near-zero determinant). |
| 58 | */ |
| 59 | #define MATD_EPS 1e-8 |
| 60 | |
| 61 | /** |
| 62 | * A macro to reference a specific matd_t data element given it's zero-based |
| 63 | * row and column indexes. Suitable for both retrieval and assignment. |
| 64 | */ |
| 65 | #define MATD_EL(m, row, col) (m)->data[((row)*(m)->ncols + (col))] |
| 66 | |
| 67 | /** |
| 68 | * Creates a double matrix with the given number of rows and columns (or a scalar |
| 69 | * in the case where rows=0 and/or cols=0). All data elements will be initialized |
| 70 | * to zero. It is the caller's responsibility to call matd_destroy() on the |
| 71 | * returned matrix. |
| 72 | */ |
| 73 | matd_t *matd_create(int rows, int cols); |
| 74 | |
| 75 | /** |
| 76 | * Creates a double matrix with the given number of rows and columns (or a scalar |
| 77 | * in the case where rows=0 and/or cols=0). All data elements will be initialized |
| 78 | * using the supplied array of data, which must contain at least rows*cols elements, |
| 79 | * arranged in row-major order (i.e. index = row*ncols + col). It is the caller's |
| 80 | * responsibility to call matd_destroy() on the returned matrix. |
| 81 | */ |
| 82 | matd_t *matd_create_data(int rows, int cols, const double *data); |
| 83 | |
| 84 | /** |
| 85 | * Creates a double matrix with the given number of rows and columns (or a scalar |
| 86 | * in the case where rows=0 and/or cols=0). All data elements will be initialized |
| 87 | * using the supplied array of float data, which must contain at least rows*cols elements, |
| 88 | * arranged in row-major order (i.e. index = row*ncols + col). It is the caller's |
| 89 | * responsibility to call matd_destroy() on the returned matrix. |
| 90 | */ |
| 91 | matd_t *matd_create_dataf(int rows, int cols, const float *data); |
| 92 | |
| 93 | /** |
| 94 | * Creates a square identity matrix with the given number of rows (and |
| 95 | * therefore columns), or a scalar with value 1 in the case where dim=0. |
| 96 | * It is the caller's responsibility to call matd_destroy() on the |
| 97 | * returned matrix. |
| 98 | */ |
| 99 | matd_t *matd_identity(int dim); |
| 100 | |
| 101 | /** |
| 102 | * Creates a scalar with the supplied value 'v'. It is the caller's responsibility |
| 103 | * to call matd_destroy() on the returned matrix. |
| 104 | * |
| 105 | * NOTE: Scalars are different than 1x1 matrices (implementation note: |
| 106 | * they are encoded as 0x0 matrices). For example: for matrices A*B, A |
| 107 | * and B must both have specific dimensions. However, if A is a |
| 108 | * scalar, there are no restrictions on the size of B. |
| 109 | */ |
| 110 | matd_t *matd_create_scalar(double v); |
| 111 | |
| 112 | /** |
| 113 | * Retrieves the cell value for matrix 'm' at the given zero-based row and column index. |
| 114 | * Performs more thorough validation checking than MATD_EL(). |
| 115 | */ |
| 116 | double matd_get(const matd_t *m, int row, int col); |
| 117 | |
| 118 | /** |
| 119 | * Assigns the given value to the matrix cell at the given zero-based row and |
| 120 | * column index. Performs more thorough validation checking than MATD_EL(). |
| 121 | */ |
| 122 | void matd_put(matd_t *m, int row, int col, double value); |
| 123 | |
| 124 | /** |
| 125 | * Retrieves the scalar value of the given element ('m' must be a scalar). |
| 126 | * Performs more thorough validation checking than MATD_EL(). |
| 127 | */ |
| 128 | double matd_get_scalar(const matd_t *m); |
| 129 | |
| 130 | /** |
| 131 | * Assigns the given value to the supplied scalar element ('m' must be a scalar). |
| 132 | * Performs more thorough validation checking than MATD_EL(). |
| 133 | */ |
| 134 | void matd_put_scalar(matd_t *m, double value); |
| 135 | |
| 136 | /** |
| 137 | * Creates an exact copy of the supplied matrix 'm'. It is the caller's |
| 138 | * responsibility to call matd_destroy() on the returned matrix. |
| 139 | */ |
| 140 | matd_t *matd_copy(const matd_t *m); |
| 141 | |
| 142 | /** |
| 143 | * Creates a copy of a subset of the supplied matrix 'a'. The subset will include |
| 144 | * rows 'r0' through 'r1', inclusive ('r1' >= 'r0'), and columns 'c0' through 'c1', |
| 145 | * inclusive ('c1' >= 'c0'). All parameters are zero-based (i.e. matd_select(a, 0, 0, 0, 0) |
| 146 | * will return only the first cell). Cannot be used on scalars or to extend |
| 147 | * beyond the number of rows/columns of 'a'. It is the caller's responsibility to |
| 148 | * call matd_destroy() on the returned matrix. |
| 149 | */ |
| 150 | matd_t *matd_select(const matd_t *a, int r0, int r1, int c0, int c1); |
| 151 | |
| 152 | /** |
| 153 | * Prints the supplied matrix 'm' to standard output by applying the supplied |
| 154 | * printf format specifier 'fmt' for each individual element. Each row will |
| 155 | * be printed on a separate newline. |
| 156 | */ |
| 157 | void matd_print(const matd_t *m, const char *fmt); |
| 158 | |
| 159 | /** |
| 160 | * Prints the transpose of the supplied matrix 'm' to standard output by applying |
| 161 | * the supplied printf format specifier 'fmt' for each individual element. Each |
| 162 | * row will be printed on a separate newline. |
| 163 | */ |
| 164 | void matd_print_transpose(const matd_t *m, const char *fmt); |
| 165 | |
| 166 | /** |
| 167 | * Adds the two supplied matrices together, cell-by-cell, and returns the results |
| 168 | * as a new matrix of the same dimensions. The supplied matrices must have |
| 169 | * identical dimensions. It is the caller's responsibility to call matd_destroy() |
| 170 | * on the returned matrix. |
| 171 | */ |
| 172 | matd_t *matd_add(const matd_t *a, const matd_t *b); |
| 173 | |
| 174 | /** |
| 175 | * Adds the values of 'b' to matrix 'a', cell-by-cell, and overwrites the |
| 176 | * contents of 'a' with the results. The supplied matrices must have |
| 177 | * identical dimensions. |
| 178 | */ |
| 179 | void matd_add_inplace(matd_t *a, const matd_t *b); |
| 180 | |
| 181 | /** |
| 182 | * Subtracts matrix 'b' from matrix 'a', cell-by-cell, and returns the results |
| 183 | * as a new matrix of the same dimensions. The supplied matrices must have |
| 184 | * identical dimensions. It is the caller's responsibility to call matd_destroy() |
| 185 | * on the returned matrix. |
| 186 | */ |
| 187 | matd_t *matd_subtract(const matd_t *a, const matd_t *b); |
| 188 | |
| 189 | /** |
| 190 | * Subtracts the values of 'b' from matrix 'a', cell-by-cell, and overwrites the |
| 191 | * contents of 'a' with the results. The supplied matrices must have |
| 192 | * identical dimensions. |
| 193 | */ |
| 194 | void matd_subtract_inplace(matd_t *a, const matd_t *b); |
| 195 | |
| 196 | /** |
| 197 | * Scales all cell values of matrix 'a' by the given scale factor 's' and |
| 198 | * returns the result as a new matrix of the same dimensions. It is the caller's |
| 199 | * responsibility to call matd_destroy() on the returned matrix. |
| 200 | */ |
| 201 | matd_t *matd_scale(const matd_t *a, double s); |
| 202 | |
| 203 | /** |
| 204 | * Scales all cell values of matrix 'a' by the given scale factor 's' and |
| 205 | * overwrites the contents of 'a' with the results. |
| 206 | */ |
| 207 | void matd_scale_inplace(matd_t *a, double s); |
| 208 | |
| 209 | /** |
| 210 | * Multiplies the two supplied matrices together (matrix product), and returns the |
| 211 | * results as a new matrix. The supplied matrices must have dimensions such that |
| 212 | * columns(a) = rows(b). The returned matrix will have a row count of rows(a) |
| 213 | * and a column count of columns(b). It is the caller's responsibility to call |
| 214 | * matd_destroy() on the returned matrix. |
| 215 | */ |
| 216 | matd_t *matd_multiply(const matd_t *a, const matd_t *b); |
| 217 | |
| 218 | /** |
| 219 | * Creates a matrix which is the transpose of the supplied matrix 'a'. It is the |
| 220 | * caller's responsibility to call matd_destroy() on the returned matrix. |
| 221 | */ |
| 222 | matd_t *matd_transpose(const matd_t *a); |
| 223 | |
| 224 | /** |
| 225 | * Calculates the determinant of the supplied matrix 'a'. |
| 226 | */ |
| 227 | double matd_det(const matd_t *a); |
| 228 | |
| 229 | /** |
| 230 | * Attempts to compute an inverse of the supplied matrix 'a' and return it as |
| 231 | * a new matrix. This is strictly only possible if the determinant of 'a' is |
| 232 | * non-zero (matd_det(a) != 0). |
| 233 | * |
| 234 | * If the determinant is zero, NULL is returned. It is otherwise the |
| 235 | * caller's responsibility to cope with the results caused by poorly |
| 236 | * conditioned matrices. (E.g.., if such a situation is likely to arise, compute |
| 237 | * the pseudo-inverse from the SVD.) |
| 238 | **/ |
| 239 | matd_t *matd_inverse(const matd_t *a); |
| 240 | |
| 241 | static inline void matd_set_data(matd_t *m, const double *data) |
| 242 | { |
| 243 | memcpy(m->data, data, m->nrows * m->ncols * sizeof(double)); |
| 244 | } |
| 245 | |
| 246 | /** |
| 247 | * Determines whether the supplied matrix 'a' is a scalar (positive return) or |
| 248 | * not (zero return, indicating a matrix of dimensions at least 1x1). |
| 249 | */ |
| 250 | static inline int matd_is_scalar(const matd_t *a) |
| 251 | { |
| 252 | assert(a != NULL); |
| 253 | return a->ncols <= 1 && a->nrows <= 1; |
| 254 | } |
| 255 | |
| 256 | /** |
| 257 | * Determines whether the supplied matrix 'a' is a row or column vector |
| 258 | * (positive return) or not (zero return, indicating either 'a' is a scalar or a |
| 259 | * matrix with at least one dimension > 1). |
| 260 | */ |
| 261 | static inline int matd_is_vector(const matd_t *a) |
| 262 | { |
| 263 | assert(a != NULL); |
| 264 | return a->ncols == 1 || a->nrows == 1; |
| 265 | } |
| 266 | |
| 267 | /** |
| 268 | * Determines whether the supplied matrix 'a' is a row or column vector |
| 269 | * with a dimension of 'len' (positive return) or not (zero return). |
| 270 | */ |
| 271 | static inline int matd_is_vector_len(const matd_t *a, int len) |
| 272 | { |
| 273 | assert(a != NULL); |
| 274 | return (a->ncols == 1 && a->nrows == (unsigned int)len) || (a->ncols == (unsigned int)len && a->nrows == 1); |
| 275 | } |
| 276 | |
| 277 | /** |
| 278 | * Calculates the magnitude of the supplied matrix 'a'. |
| 279 | */ |
| 280 | double matd_vec_mag(const matd_t *a); |
| 281 | |
| 282 | /** |
| 283 | * Calculates the magnitude of the distance between the points represented by |
| 284 | * matrices 'a' and 'b'. Both 'a' and 'b' must be vectors and have the same |
| 285 | * dimension (although one may be a row vector and one may be a column vector). |
| 286 | */ |
| 287 | double matd_vec_dist(const matd_t *a, const matd_t *b); |
| 288 | |
| 289 | |
| 290 | /** |
| 291 | * Same as matd_vec_dist, but only uses the first 'n' terms to compute distance |
| 292 | */ |
| 293 | double matd_vec_dist_n(const matd_t *a, const matd_t *b, int n); |
| 294 | |
| 295 | /** |
| 296 | * Calculates the dot product of two vectors. Both 'a' and 'b' must be vectors |
| 297 | * and have the same dimension (although one may be a row vector and one may be |
| 298 | * a column vector). |
| 299 | */ |
| 300 | double matd_vec_dot_product(const matd_t *a, const matd_t *b); |
| 301 | |
| 302 | /** |
| 303 | * Calculates the normalization of the supplied vector 'a' (i.e. a unit vector |
| 304 | * of the same dimension and orientation as 'a' with a magnitude of 1) and returns |
| 305 | * it as a new vector. 'a' must be a vector of any dimension and must have a |
| 306 | * non-zero magnitude. It is the caller's responsibility to call matd_destroy() |
| 307 | * on the returned matrix. |
| 308 | */ |
| 309 | matd_t *matd_vec_normalize(const matd_t *a); |
| 310 | |
| 311 | /** |
| 312 | * Calculates the cross product of supplied matrices 'a' and 'b' (i.e. a x b) |
| 313 | * and returns it as a new matrix. Both 'a' and 'b' must be vectors of dimension |
| 314 | * 3, but can be either row or column vectors. It is the caller's responsibility |
| 315 | * to call matd_destroy() on the returned matrix. |
| 316 | */ |
| 317 | matd_t *matd_crossproduct(const matd_t *a, const matd_t *b); |
| 318 | |
| 319 | double matd_err_inf(const matd_t *a, const matd_t *b); |
| 320 | |
| 321 | /** |
| 322 | * Creates a new matrix by applying a series of matrix operations, as expressed |
| 323 | * in 'expr', to the supplied list of matrices. Each matrix to be operated upon |
| 324 | * must be represented in the expression by a separate matrix placeholder, 'M', |
| 325 | * and there must be one matrix supplied as an argument for each matrix |
| 326 | * placeholder in the expression. All rules and caveats of the corresponding |
| 327 | * matrix operations apply to the operated-on matrices. It is the caller's |
| 328 | * responsibility to call matd_destroy() on the returned matrix. |
| 329 | * |
| 330 | * Available operators (in order of increasing precedence): |
| 331 | * M+M add two matrices together |
| 332 | * M-M subtract one matrix from another |
| 333 | * M*M multiply two matrices together (matrix product) |
| 334 | * MM multiply two matrices together (matrix product) |
| 335 | * -M negate a matrix |
| 336 | * M^-1 take the inverse of a matrix |
| 337 | * M' take the transpose of a matrix |
| 338 | * |
| 339 | * Expressions can be combined together and grouped by enclosing them in |
| 340 | * parenthesis, i.e.: |
| 341 | * -M(M+M+M)-(M*M)^-1 |
| 342 | * |
| 343 | * Scalar values can be generated on-the-fly, i.e.: |
| 344 | * M*2.2 scales M by 2.2 |
| 345 | * -2+M adds -2 to all elements of M |
| 346 | * |
| 347 | * All whitespace in the expression is ignored. |
| 348 | */ |
| 349 | matd_t *matd_op(const char *expr, ...); |
| 350 | |
| 351 | /** |
| 352 | * Frees the memory associated with matrix 'm', being the result of an earlier |
| 353 | * call to a matd_*() function, after which 'm' will no longer be usable. |
| 354 | */ |
| 355 | void matd_destroy(matd_t *m); |
| 356 | |
| 357 | typedef struct |
| 358 | { |
| 359 | matd_t *U; |
| 360 | matd_t *S; |
| 361 | matd_t *V; |
| 362 | } matd_svd_t; |
| 363 | |
| 364 | /** Compute a complete SVD of a matrix. The SVD exists for all |
| 365 | * matrices. For a matrix MxN, we will have: |
| 366 | * |
| 367 | * A = U*S*V' |
| 368 | * |
| 369 | * where A is MxN, U is MxM (and is an orthonormal basis), S is MxN |
| 370 | * (and is diagonal up to machine precision), and V is NxN (and is an |
| 371 | * orthonormal basis). |
| 372 | * |
| 373 | * The caller is responsible for destroying U, S, and V. |
| 374 | **/ |
| 375 | matd_svd_t matd_svd(matd_t *A); |
| 376 | |
| 377 | #define MATD_SVD_NO_WARNINGS 1 |
| 378 | matd_svd_t matd_svd_flags(matd_t *A, int flags); |
| 379 | |
| 380 | //////////////////////////////// |
| 381 | // PLU Decomposition |
| 382 | |
| 383 | // All square matrices (even singular ones) have a partially-pivoted |
| 384 | // LU decomposition such that A = PLU, where P is a permutation |
| 385 | // matrix, L is a lower triangular matrix, and U is an upper |
| 386 | // triangular matrix. |
| 387 | // |
| 388 | typedef struct |
| 389 | { |
| 390 | // was the input matrix singular? When a zero pivot is found, this |
| 391 | // flag is set to indicate that this has happened. |
| 392 | int singular; |
| 393 | |
| 394 | unsigned int *piv; // permutation indices |
| 395 | int pivsign; // either +1 or -1 |
| 396 | |
| 397 | // The matd_plu_t object returned "owns" the enclosed LU matrix. It |
| 398 | // is not expected that the returned object is itself useful to |
| 399 | // users: it contains the L and U information all smushed |
| 400 | // together. |
| 401 | matd_t *lu; // combined L and U matrices, permuted so they can be triangular. |
| 402 | } matd_plu_t; |
| 403 | |
| 404 | matd_plu_t *matd_plu(const matd_t *a); |
| 405 | void matd_plu_destroy(matd_plu_t *mlu); |
| 406 | double matd_plu_det(const matd_plu_t *lu); |
| 407 | matd_t *matd_plu_p(const matd_plu_t *lu); |
| 408 | matd_t *matd_plu_l(const matd_plu_t *lu); |
| 409 | matd_t *matd_plu_u(const matd_plu_t *lu); |
| 410 | matd_t *matd_plu_solve(const matd_plu_t *mlu, const matd_t *b); |
| 411 | |
| 412 | // uses LU decomposition internally. |
| 413 | matd_t *matd_solve(matd_t *A, matd_t *b); |
| 414 | |
| 415 | //////////////////////////////// |
| 416 | // Cholesky Factorization |
| 417 | |
| 418 | /** |
| 419 | * Creates a double matrix with the Cholesky lower triangular matrix |
| 420 | * of A. A must be symmetric, positive definite. It is the caller's |
| 421 | * responsibility to call matd_destroy() on the returned matrix. |
| 422 | */ |
| 423 | //matd_t *matd_cholesky(const matd_t *A); |
| 424 | |
| 425 | typedef struct |
| 426 | { |
| 427 | int is_spd; |
| 428 | matd_t *u; |
| 429 | } matd_chol_t; |
| 430 | |
| 431 | matd_chol_t *matd_chol(matd_t *A); |
| 432 | matd_t *matd_chol_solve(const matd_chol_t *chol, const matd_t *b); |
| 433 | void matd_chol_destroy(matd_chol_t *chol); |
| 434 | // only sensible on PSD matrices |
| 435 | matd_t *matd_chol_inverse(matd_t *a); |
| 436 | |
| 437 | void matd_ltransposetriangle_solve(matd_t *u, const double *b, double *x); |
| 438 | void matd_ltriangle_solve(matd_t *u, const double *b, double *x); |
| 439 | void matd_utriangle_solve(matd_t *u, const double *b, double *x); |
| 440 | |
| 441 | |
| 442 | double matd_max(matd_t *m); |
| 443 | |
| 444 | #ifdef __cplusplus |
| 445 | } |
| 446 | #endif |