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 // Computes the determinant of a 3 by 3 matrix. 129 float mat33Determinant(const struct Mat33 *A); 130 131 // 4x4 MATRIX MATH ///////////////////////////////////////////////////////////// 132 // Updates out with the multiplication of A and v, i.e.: 133 // out = Av. 134 void mat44Apply(struct Vec4 *out, const struct Mat44 *A, const struct Vec4 *v); 135 136 // Decomposes the given matrix LU inplace, such that: 137 // LU = P' * L * U. 138 // where L is a lower-diagonal matrix, U is an upper-diagonal matrix, and P is a 139 // permutation matrix. 140 // 141 // L and U are stored compactly in the returned LU matrix such that: 142 // -) the superdiagonal elements make up "U" (with a diagonal of 1.0s), 143 // -) the subdiagonal and diagonal elements make up "L". 144 // e.g. if the returned LU matrix is: 145 // LU = [A11 A12 A13 A14 146 // A21 A22 A23 A24 147 // A31 A32 A33 A34 148 // A41 A42 A43 A44], then: 149 // L = [A11 0 0 0 and U = [ 1 A12 A13 A14 150 // A21 A22 0 0 0 1 A23 A24 151 // A31 A32 A33 0 0 0 1 A34 152 // A41 A42 A43 A44] 0 0 0 1 ] 153 // 154 // The permutation matrix P can be reproduced from returned pivot vector as: 155 // matrix P(N); 156 // P.identity(); 157 // for (size_t i = 0; i < N; ++i) { 158 // P.swapRows(i, pivot[i]); 159 // } 160 void mat44DecomposeLup(struct Mat44 *LU, struct Size4 *pivot); 161 162 // Solves the linear system A x = b for x, where A is a compact LU decomposition 163 // (i.e. the LU matrix from mat44DecomposeLup) and pivot is the corresponding 164 // row pivots for the permutation matrix (also from mat44DecomposeLup). 165 void mat44Solve(const struct Mat44 *A, struct Vec4 *x, const struct Vec4 *b, 166 const struct Size4 *pivot); 167 168 // MXN MATRIX MATH ///////////////////////////////////////////////////////////// 169 /* 170 * The following functions define basic math functionality for matrices of 171 * arbitrary dimension. 172 * 173 * All matrices used in these functions are assumed to be row major, i.e. if: 174 * A = [1 2 3 175 * 4 5 6 176 * 7 8 9] 177 * then when A is passed into one of the functions below, the order of 178 * elements is assumed to be [1 2 3 4 5 6 7 8 9]. 179 */ 180 181 // Returns the maximum diagonal element of the given matrix. 182 // The matrix is assumed to be square, of size n x n. 183 float matMaxDiagonalElement(const float *square_mat, size_t n); 184 185 // Adds a constant value to the diagonal of the given square n x n matrix and 186 // returns the updated matrix in place: 187 // A = A + uI 188 void matAddConstantDiagonal(float *square_mat, float u, size_t n); 189 190 // Updates out with the result of A's transpose multiplied with A (i.e. A^T A). 191 // A is a matrix with dimensions nrows x ncols. 192 // out is a matrix with dimensions ncols x ncols. 193 void matTransposeMultiplyMat(float *out, const float *A, 194 size_t nrows, size_t ncols); 195 196 // Updates out with the result of A's transpose multiplied with v (i.e. A^T v). 197 // A is a matrix with dimensions nrows x ncols. 198 // v is a vector of dimension nrows. 199 // out is a vector of dimension ncols. 200 void matTransposeMultiplyVec(float* out, const float *A, const float *v, 201 size_t nrows, size_t ncols); 202 203 // Updates out with the result of A multiplied with v (i.e. out = Av). 204 // A is a matrix with dimensions nrows x ncols. 205 // v is a vector of dimension ncols. 206 // out is a vector of dimension nrows. 207 void matMultiplyVec(float *out, const float *A, const float *v, 208 size_t nrows, size_t ncols); 209 210 // Solves the linear system L L^T x = b for x, where L is a lower diagonal, 211 // symmetric matrix, i.e. the Cholesky factor of a matrix A = L L^T. 212 // L is a lower-diagonal matrix of dimension n x n. 213 // b is a vector of dimension n. 214 // x is a vector of dimension n. 215 // Returns true if the solver succeeds. 216 bool matLinearSolveCholesky(float *x, const float *L, const float *b, 217 size_t n); 218 219 // Performs the Cholesky decomposition on the given matrix A such that: 220 // A = L L^T, where L, the Cholesky factor, is a lower diagonal matrix. 221 // Updates the provided L matrix with the Cholesky factor. 222 // This decomposition is only successful for symmetric, positive definite 223 // matrices A. 224 // Returns true if the solver succeeds (will fail if the matrix is not 225 // symmetric, positive definite). 226 bool matCholeskyDecomposition(float *L, const float *A, size_t n); 227 228 #ifdef __cplusplus 229 } 230 #endif 231 232 #endif // LOCATION_LBS_CONTEXTHUB_NANOAPPS_COMMON_MATH_MAT_H_ 233
