1 /* 2 * Copyright (C) 2016 The Android Open Source Project 3 * 4 * Licensed under the Apache License, Version 2.0 (the "License"); 5 * you may not use this file except in compliance with the License. 6 * You may obtain a copy of the License at 7 * 8 * http://www.apache.org/licenses/LICENSE-2.0 9 * 10 * Unless required by applicable law or agreed to in writing, software 11 * distributed under the License is distributed on an "AS IS" BASIS, 12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 * See the License for the specific language governing permissions and 14 * limitations under the License. 15 */ 16 ///////////////////////////////////////////////////////////////////////// 17 /* 18 * This module contains matrix math utilities for the following datatypes: 19 * -) Mat33 structures for 3x3 dimensional matrices 20 * -) Mat44 structures for 4x4 dimensional matrices 21 * -) floating point arrays for NxM dimensional matrices. 22 * 23 * Note that the Mat33 and Mat44 utilities were ported from the Android 24 * repository and maintain dependencies in that separate codebase. As a 25 * result, the function signatures were left untouched for compatibility with 26 * this legacy code, despite certain style violations. In particular, for this 27 * module the function argument ordering is outputs before inputs. This style 28 * violation will be addressed once the full set of dependencies in Android 29 * have been brought into this repository. 30 */ 31 #ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_COMMON_MATH_MAT_H_ 32 #define LOCATION_LBS_CONTEXTHUB_NANOAPPS_COMMON_MATH_MAT_H_ 33 34 #include <stdbool.h> 35 #include <stddef.h> 36 #include <stdint.h> 37 38 #include "common/math/vec.h" 39 40 #ifdef __cplusplus 41 extern "C" { 42 #endif 43 44 struct Mat33 { 45 float elem[3][3]; 46 }; 47 48 struct Size3 { 49 uint32_t elem[3]; 50 }; 51 52 struct Mat44 { 53 float elem[4][4]; 54 }; 55 56 struct Size4 { 57 uint32_t elem[4]; 58 }; 59 60 // 3x3 MATRIX MATH ///////////////////////////////////////////////////////////// 61 void initZeroMatrix(struct Mat33 *A); 62 63 // Updates A with the value x on the main diagonal and 0 on the off diagonals, 64 // i.e.: 65 // A = [x 0 0 66 // 0 x 0 67 // 0 0 x] 68 void initDiagonalMatrix(struct Mat33 *A, float x); 69 70 // Updates A such that the columns are given by the provided vectors, i.e.: 71 // A = [v1 v2 v3]. 72 void initMatrixColumns(struct Mat33 *A, const struct Vec3 *v1, 73 const struct Vec3 *v2, const struct Vec3 *v3); 74 75 // Updates out with the multiplication of A with v, i.e.: 76 // out = A v. 77 void mat33Apply(struct Vec3 *out, const struct Mat33 *A, const struct Vec3 *v); 78 79 // Updates out with the multiplication of A with B, i.e.: 80 // out = A B. 81 void mat33Multiply(struct Mat33 *out, const struct Mat33 *A, 82 const struct Mat33 *B); 83 84 // Updates A by scaling all entries by the provided scalar c, i.e.: 85 // A = A c. 86 void mat33ScalarMul(struct Mat33 *A, float c); 87 88 // Updates out by adding A to out, i.e.: 89 // out = out + A. 90 void mat33Add(struct Mat33 *out, const struct Mat33 *A); 91 92 // Updates out by subtracting A from out, i.e.: 93 // out = out - A. 94 void mat33Sub(struct Mat33 *out, const struct Mat33 *A); 95 96 // Returns 1 if the minimum eigenvalue of the matrix A is greater than the 97 // given tolerance. Note that the tolerance is assumed to be greater than 0. 98 // I.e., returns: 1[min(eig(A)) > tolerance]. 99 // NOTE: this function currently only checks matrix symmetry and positivity 100 // of the diagonals which is insufficient for testing positive semidefinite. 101 int mat33IsPositiveSemidefinite(const struct Mat33 *A, float tolerance); 102 103 // Updates out with the inverse of the matrix A, i.e.: 104 // out = A^(-1) 105 void mat33Invert(struct Mat33 *out, const struct Mat33 *A); 106 107 // Updates out with the multiplication of A's transpose with B, i.e.: 108 // out = A^T B 109 void mat33MultiplyTransposed(struct Mat33 *out, const struct Mat33 *A, 110 const struct Mat33 *B); 111 112 // Updates out with the multiplication of A with B's transpose, i.e.: 113 // out = A B^T 114 void mat33MultiplyTransposed2(struct Mat33 *out, const struct Mat33 *A, 115 const struct Mat33 *B); 116 117 // Updates out with the transpose of A, i.e.: 118 // out = A^T 119 void mat33Transpose(struct Mat33 *out, const struct Mat33 *A); 120 121 // Returns the eigenvalues and corresponding eigenvectors of the symmetric 122 // matrix S. 123 // The i-th eigenvalue corresponds to the eigenvector in the i-th row of 124 // the matrix eigenvecs. 125 void mat33GetEigenbasis(struct Mat33 *S, struct Vec3 *eigenvals, 126 struct Mat33 *eigenvecs); 127 128 129 // 4x4 MATRIX MATH ///////////////////////////////////////////////////////////// 130 // Updates out with the multiplication of A and v, i.e.: 131 // out = Av. 132 void mat44Apply(struct Vec4 *out, const struct Mat44 *A, const struct Vec4 *v); 133 134 // Decomposes the given matrix LU inplace, such that: 135 // LU = P' * L * U. 136 // where L is a lower-diagonal matrix, U is an upper-diagonal matrix, and P is a 137 // permutation matrix. 138 // 139 // L and U are stored compactly in the returned LU matrix such that: 140 // -) the superdiagonal elements make up "U" (with a diagonal of 1.0s), 141 // -) the subdiagonal and diagonal elements make up "L". 142 // e.g. if the returned LU matrix is: 143 // LU = [A11 A12 A13 A14 144 // A21 A22 A23 A24 145 // A31 A32 A33 A34 146 // A41 A42 A43 A44], then: 147 // L = [A11 0 0 0 and U = [ 1 A12 A13 A14 148 // A21 A22 0 0 0 1 A23 A24 149 // A31 A32 A33 0 0 0 1 A34 150 // A41 A42 A43 A44] 0 0 0 1 ] 151 // 152 // The permutation matrix P can be reproduced from returned pivot vector as: 153 // matrix P(N); 154 // P.identity(); 155 // for (size_t i = 0; i < N; ++i) { 156 // P.swapRows(i, pivot[i]); 157 // } 158 void mat44DecomposeLup(struct Mat44 *LU, struct Size4 *pivot); 159 160 // Solves the linear system A x = b for x, where A is a compact LU decomposition 161 // (i.e. the LU matrix from mat44DecomposeLup) and pivot is the corresponding 162 // row pivots for the permutation matrix (also from mat44DecomposeLup). 163 void mat44Solve(const struct Mat44 *A, struct Vec4 *x, const struct Vec4 *b, 164 const struct Size4 *pivot); 165 166 // MXN MATRIX MATH ///////////////////////////////////////////////////////////// 167 /* 168 * The following functions define basic math functionality for matrices of 169 * arbitrary dimension. 170 * 171 * All matrices used in these functions are assumed to be row major, i.e. if: 172 * A = [1 2 3 173 * 4 5 6 174 * 7 8 9] 175 * then when A is passed into one of the functions below, the order of 176 * elements is assumed to be [1 2 3 4 5 6 7 8 9]. 177 */ 178 179 // Returns the maximum diagonal element of the given matrix. 180 // The matrix is assumed to be square, of size n x n. 181 float matMaxDiagonalElement(const float *square_mat, size_t n); 182 183 // Adds a constant value to the diagonal of the given square n x n matrix and 184 // returns the updated matrix in place: 185 // A = A + uI 186 void matAddConstantDiagonal(float *square_mat, float u, size_t n); 187 188 // Updates out with the result of A's transpose multiplied with A (i.e. A^T A). 189 // A is a matrix with dimensions nrows x ncols. 190 // out is a matrix with dimensions ncols x ncols. 191 void matTransposeMultiplyMat(float *out, const float *A, 192 size_t nrows, size_t ncols); 193 194 // Updates out with the result of A's transpose multiplied with v (i.e. A^T v). 195 // A is a matrix with dimensions nrows x ncols. 196 // v is a vector of dimension nrows. 197 // out is a vector of dimension ncols. 198 void matTransposeMultiplyVec(float* out, const float *A, const float *v, 199 size_t nrows, size_t ncols); 200 201 // Updates out with the result of A multiplied with v (i.e. out = Av). 202 // A is a matrix with dimensions nrows x ncols. 203 // v is a vector of dimension ncols. 204 // out is a vector of dimension nrows. 205 void matMultiplyVec(float *out, const float *A, const float *v, 206 size_t nrows, size_t ncols); 207 208 // Solves the linear system L L^T x = b for x, where L is a lower diagonal, 209 // symmetric matrix, i.e. the Cholesky factor of a matrix A = L L^T. 210 // L is a lower-diagonal matrix of dimension n x n. 211 // b is a vector of dimension n. 212 // x is a vector of dimension n. 213 // Returns true if the solver succeeds. 214 bool matLinearSolveCholesky(float *x, const float *L, const float *b, 215 size_t n); 216 217 // Performs the Cholesky decomposition on the given matrix A such that: 218 // A = L L^T, where L, the Cholesky factor, is a lower diagonal matrix. 219 // Updates the provided L matrix with the Cholesky factor. 220 // This decomposition is only successful for symmetric, positive definite 221 // matrices A. 222 // Returns true if the solver succeeds (will fail if the matrix is not 223 // symmetric, positive definite). 224 bool matCholeskyDecomposition(float *L, const float *A, size_t n); 225 226 #ifdef __cplusplus 227 } 228 #endif 229 230 #endif // LOCATION_LBS_CONTEXTHUB_NANOAPPS_COMMON_MATH_MAT_H_ 231