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 // Note: Keep this code to preserve Android codebase dependencies.
49 struct Size3 {
50   uint32_t elem[3];
51 };
52 
53 struct Mat44 {
54   float elem[4][4];
55 };
56 
57 struct Size4 {
58   uint32_t elem[4];
59 };
60 
61 // 3x3 MATRIX MATH /////////////////////////////////////////////////////////////
62 void initZeroMatrix(struct Mat33 *A);
63 
64 // Updates A with the value x on the main diagonal and 0 on the off diagonals,
65 // i.e.:
66 // A = [x 0 0
67 //      0 x 0
68 //      0 0 x]
69 void initDiagonalMatrix(struct Mat33 *A, float x);
70 
71 // Updates A such that the columns are given by the provided vectors, i.e.:
72 // A = [v1 v2 v3].
73 void initMatrixColumns(struct Mat33 *A, const struct Vec3 *v1,
74                        const struct Vec3 *v2, const struct Vec3 *v3);
75 
76 // Updates out with the multiplication of A with v, i.e.:
77 // out = A v.
78 void mat33Apply(struct Vec3 *out, const struct Mat33 *A, const struct Vec3 *v);
79 
80 // Updates out with the multiplication of A with B, i.e.:
81 // out =  A B.
82 void mat33Multiply(struct Mat33 *out, const struct Mat33 *A,
83                    const struct Mat33 *B);
84 
85 // Updates A by scaling all entries by the provided scalar c, i.e.:
86 // A = A c.
87 void mat33ScalarMul(struct Mat33 *A, float c);
88 
89 // Updates out by adding A to out, i.e.:
90 // out = out + A.
91 void mat33Add(struct Mat33 *out, const struct Mat33 *A);
92 
93 // Updates out by subtracting A from out, i.e.:
94 // out = out - A.
95 void mat33Sub(struct Mat33 *out, const struct Mat33 *A);
96 
97 // Returns 1 if the minimum eigenvalue of the matrix A is greater than the
98 // given tolerance. Note that the tolerance is assumed to be greater than 0.
99 // I.e., returns: 1[min(eig(A)) > tolerance].
100 // NOTE: this function currently only checks matrix symmetry and positivity
101 // of the diagonals which is insufficient for testing positive semidefinite.
102 int mat33IsPositiveSemidefinite(const struct Mat33 *A, float tolerance);
103 
104 // Updates out with the inverse of the matrix A, i.e.:
105 // out = A^(-1)
106 void mat33Invert(struct Mat33 *out, const struct Mat33 *A);
107 
108 // Updates out with the multiplication of A's transpose with B, i.e.:
109 // out = A^T B
110 void mat33MultiplyTransposed(struct Mat33 *out, const struct Mat33 *A,
111                              const struct Mat33 *B);
112 
113 // Note: Keep this code to preserve Android codebase dependencies.
114 // Updates out with the multiplication of A with B's transpose, i.e.:
115 // out = A B^T
116 void mat33MultiplyTransposed2(struct Mat33 *out, const struct Mat33 *A,
117                               const struct Mat33 *B);
118 
119 // Updates out with the transpose of A, i.e.:
120 // out = A^T
121 void mat33Transpose(struct Mat33 *out, const struct Mat33 *A);
122 
123 // Returns the eigenvalues and corresponding eigenvectors of the symmetric
124 // matrix S.
125 // The i-th eigenvalue corresponds to the eigenvector in the i-th row of
126 // the matrix eigenvecs.
127 void mat33GetEigenbasis(struct Mat33 *S, struct Vec3 *eigenvals,
128                         struct Mat33 *eigenvecs);
129 
130 // Computes the determinant of a 3 by 3 matrix.
131 float mat33Determinant(const struct Mat33 *A);
132 
133 // 4x4 MATRIX MATH /////////////////////////////////////////////////////////////
134 // Updates out with the multiplication of A and v, i.e.:
135 // out = Av.
136 void mat44Apply(struct Vec4 *out, const struct Mat44 *A, const struct Vec4 *v);
137 
138 // Decomposes the given matrix LU inplace, such that:
139 // LU = P' * L * U.
140 // where L is a lower-diagonal matrix, U is an upper-diagonal matrix, and P is a
141 // permutation matrix.
142 //
143 // L and U are stored compactly in the returned LU matrix such that:
144 // -) the superdiagonal elements make up "U" (with a diagonal of 1.0s),
145 // -) the subdiagonal and diagonal elements make up "L".
146 // e.g. if the returned LU matrix is:
147 //      LU = [A11 A12 A13 A14
148 //            A21 A22 A23 A24
149 //            A31 A32 A33 A34
150 //            A41 A42 A43 A44], then:
151 //       L = [A11  0   0   0      and   U = [ 1  A12 A13 A14
152 //            A21 A22  0   0                  0   1  A23 A24
153 //            A31 A32 A33  0                  0   0   1  A34
154 //            A41 A42 A43 A44]                0   0   0   1 ]
155 //
156 // The permutation matrix P can be reproduced from returned pivot vector as:
157 // matrix P(N);
158 // P.identity();
159 // for (size_t i = 0; i < N; ++i) {
160 //    P.swapRows(i, pivot[i]);
161 // }
162 void mat44DecomposeLup(struct Mat44 *LU, struct Size4 *pivot);
163 
164 // Solves the linear system A x = b for x, where A is a compact LU decomposition
165 // (i.e. the LU matrix from mat44DecomposeLup) and pivot is the corresponding
166 // row pivots for the permutation matrix (also from mat44DecomposeLup).
167 void mat44Solve(const struct Mat44 *A, struct Vec4 *x, const struct Vec4 *b,
168                 const struct Size4 *pivot);
169 
170 // MXN MATRIX MATH /////////////////////////////////////////////////////////////
171 /*
172  * The following functions define basic math functionality for matrices of
173  * arbitrary dimension.
174  *
175  * All matrices used in these functions are assumed to be row major, i.e. if:
176  * A = [1 2 3
177  *      4 5 6
178  *      7 8 9]
179  * then when A is passed into one of the functions below, the order of
180  * elements is assumed to be [1 2 3 4 5 6 7 8 9].
181  */
182 
183 // Returns the maximum diagonal element of the given matrix.
184 // The matrix is assumed to be square, of size n x n.
185 float matMaxDiagonalElement(const float *square_mat, size_t n);
186 
187 // Adds a constant value to the diagonal of the given square n x n matrix and
188 // returns the updated matrix in place:
189 // A = A + uI
190 void matAddConstantDiagonal(float *square_mat, float u, size_t n);
191 
192 // Updates out with the result of A's transpose multiplied with A (i.e. A^T A).
193 // A is a matrix with dimensions nrows x ncols.
194 // out is a matrix with dimensions ncols x ncols.
195 void matTransposeMultiplyMat(float *out, const float *A,
196                              size_t nrows, size_t ncols);
197 
198 // Updates out with the result of A's transpose multiplied with v (i.e. A^T v).
199 // A is a matrix with dimensions nrows x ncols.
200 // v is a vector of dimension nrows.
201 // out is a vector of dimension ncols.
202 void matTransposeMultiplyVec(float* out, const float *A, const float *v,
203                              size_t nrows, size_t ncols);
204 
205 // Updates out with the result of A multiplied with v (i.e. out = Av).
206 // A is a matrix with dimensions nrows x ncols.
207 // v is a vector of dimension ncols.
208 // out is a vector of dimension nrows.
209 void matMultiplyVec(float *out, const float *A, const float *v,
210                     size_t nrows, size_t ncols);
211 
212 // Solves the linear system L L^T x = b for x, where L is a lower diagonal,
213 // symmetric matrix, i.e. the Cholesky factor of a matrix A = L L^T.
214 // L is a lower-diagonal matrix of dimension n x n.
215 // b is a vector of dimension n.
216 // x is a vector of dimension n.
217 // Returns true if the solver succeeds.
218 bool matLinearSolveCholesky(float *x, const float *L, const float *b,
219                             size_t n);
220 
221 // Performs the Cholesky decomposition on the given matrix A such that:
222 // A = L L^T, where L, the Cholesky factor, is a lower diagonal matrix.
223 // Updates the provided L matrix with the Cholesky factor.
224 // This decomposition is only successful for symmetric, positive definite
225 // matrices A.
226 // Returns true if the solver succeeds (will fail if the matrix is not
227 // symmetric, positive definite).
228 bool matCholeskyDecomposition(float *L, const float *A, size_t n);
229 
230 #ifdef __cplusplus
231 }
232 #endif
233 
234 #endif  // LOCATION_LBS_CONTEXTHUB_NANOAPPS_COMMON_MATH_MAT_H_
235