Line data Source code
1 : /*
2 : * matrix3.cpp
3 : * Copyright (C) Siddharth Bharat Purohit, 3DRobotics Inc. 2015
4 : *
5 : * This file is free software: you can redistribute it and/or modify it
6 : * under the terms of the GNU General Public License as published by the
7 : * Free Software Foundation, either version 3 of the License, or
8 : * (at your option) any later version.
9 : *
10 : * This file is distributed in the hope that it will be useful, but
11 : * WITHOUT ANY WARRANTY; without even the implied warranty of
12 : * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
13 : * See the GNU General Public License for more details.
14 : *
15 : * You should have received a copy of the GNU General Public License along
16 : * with this program. If not, see <http://www.gnu.org/licenses/>.
17 : */
18 :
19 : #include <AP_HAL/AP_HAL.h>
20 : #include "AP_Math.h"
21 :
22 : #include <stdio.h>
23 : #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
24 : #include <fenv.h>
25 : #endif
26 :
27 : /*
28 : * Does matrix multiplication of two regular/square matrices
29 : *
30 : * @param A, Matrix A
31 : * @param B, Matrix B
32 : * @param n, dimemsion of square matrices
33 : * @returns multiplied matrix i.e. A*B
34 : */
35 : template<typename T>
36 2820 : static T* matrix_multiply(const T *A, const T *B, uint16_t n)
37 : {
38 2820 : T* ret = NEW_NOTHROW T[n*n];
39 2820 : memset(ret,0.0f,n*n*sizeof(T));
40 :
41 27300 : for(uint16_t i = 0; i < n; i++) {
42 239400 : for(uint16_t j = 0; j < n; j++) {
43 2116800 : for(uint16_t k = 0;k < n; k++) {
44 1901880 : ret[i*n + j] += A[i*n + k] * B[k*n + j];
45 : }
46 : }
47 : }
48 2820 : return ret;
49 : }
50 :
51 : template<typename T>
52 11958 : static inline void swap(T &a, T &b)
53 : {
54 : T c;
55 11958 : c = a;
56 11958 : a = b;
57 11958 : b = c;
58 11958 : }
59 :
60 : /*
61 : * calculates pivot matrix such that all the larger elements in the row are on diagonal
62 : *
63 : * @param A, input matrix matrix
64 : * @param pivot
65 : * @param n, dimenstion of square matrix
66 : * @returns false = matrix is Singular or non positive definite, true = matrix inversion successful
67 : */
68 : template<typename T>
69 940 : static void mat_pivot(const T* A, T* pivot, uint16_t n)
70 : {
71 9100 : for(uint16_t i = 0;i<n;i++){
72 79800 : for(uint16_t j=0;j<n;j++) {
73 71640 : pivot[i*n+j] = static_cast<T>(i==j);
74 : }
75 : }
76 :
77 9100 : for(uint16_t i = 0;i < n; i++) {
78 8160 : uint16_t max_j = i;
79 48060 : for(uint16_t j=i;j<n;j++){
80 39900 : if(fabsF(A[j*n + i]) > fabsF(A[max_j*n + i])) {
81 3222 : max_j = j;
82 : }
83 : }
84 :
85 8160 : if(max_j != i) {
86 13288 : for(uint16_t k = 0; k < n; k++) {
87 11958 : swap(pivot[i*n + k], pivot[max_j*n + k]);
88 : }
89 : }
90 : }
91 940 : }
92 :
93 : /*
94 : * calculates matrix inverse of Lower trangular matrix using forward substitution
95 : *
96 : * @param L, lower triangular matrix
97 : * @param out, Output inverted lower triangular matrix
98 : * @param n, dimension of matrix
99 : */
100 : template<typename T>
101 940 : static void mat_forward_sub(const T *L, T *out, uint16_t n)
102 : {
103 : // Forward substitution solve LY = I
104 9100 : for(int i = 0; i < n; i++) {
105 8160 : out[i*n + i] = 1/L[i*n + i];
106 39900 : for (int j = i+1; j < n; j++) {
107 136040 : for (int k = i; k < j; k++) {
108 104300 : out[j*n + i] -= L[j*n + k] * out[k*n + i];
109 : }
110 31740 : out[j*n + i] /= L[j*n + j];
111 : }
112 : }
113 940 : }
114 :
115 : /*
116 : * calculates matrix inverse of Upper trangular matrix using backward substitution
117 : *
118 : * @param U, upper triangular matrix
119 : * @param out, Output inverted upper triangular matrix
120 : * @param n, dimension of matrix
121 : */
122 : template<typename T>
123 940 : static void mat_back_sub(const T *U, T *out, uint16_t n)
124 : {
125 : // Backward Substitution solve UY = I
126 9100 : for(int i = n-1; i >= 0; i--) {
127 8160 : out[i*n + i] = 1/U[i*n + i];
128 39900 : for (int j = i - 1; j >= 0; j--) {
129 136040 : for (int k = i; k > j; k--) {
130 104300 : out[j*n + i] -= U[j*n + k] * out[k*n + i];
131 : }
132 31740 : out[j*n + i] /= U[j*n + j];
133 : }
134 : }
135 940 : }
136 :
137 : /*
138 : * Decomposes square matrix into Lower and Upper triangular matrices such that
139 : * A*P = L*U, where P is the pivot matrix
140 : * ref: http://rosettacode.org/wiki/LU_decomposition
141 : * @param U, upper triangular matrix
142 : * @param out, Output inverted upper triangular matrix
143 : * @param n, dimension of matrix
144 : */
145 : template<typename T>
146 940 : static void mat_LU_decompose(const T* A, T* L, T* U, T *P, uint16_t n)
147 : {
148 940 : memset(L,0,n*n*sizeof(T));
149 940 : memset(U,0,n*n*sizeof(T));
150 940 : memset(P,0,n*n*sizeof(T));
151 940 : mat_pivot(A,P,n);
152 :
153 940 : T *APrime = matrix_multiply(P,A,n);
154 9100 : for(uint16_t i = 0; i < n; i++) {
155 8160 : L[i*n + i] = 1;
156 : }
157 9100 : for(uint16_t i = 0; i < n; i++) {
158 79800 : for(uint16_t j = 0; j < n; j++) {
159 71640 : if(j <= i) {
160 39900 : U[j*n + i] = APrime[j*n + i];
161 144200 : for(uint16_t k = 0; k < j; k++) {
162 104300 : U[j*n + i] -= L[j*n + k] * U[k*n + i];
163 : }
164 : }
165 71640 : if(j >= i) {
166 39900 : L[j*n + i] = APrime[j*n + i];
167 144200 : for(uint16_t k = 0; k < i; k++) {
168 104300 : L[j*n + i] -= L[j*n + k] * U[k*n + i];
169 : }
170 39900 : L[j*n + i] /= U[i*n + i];
171 : }
172 : }
173 : }
174 940 : delete[] APrime;
175 940 : }
176 :
177 : /*
178 : * matrix inverse code for any square matrix using LU decomposition
179 : * inv = inv(U)*inv(L)*P, where L and U are triagular matrices and P the pivot matrix
180 : * ref: http://www.cl.cam.ac.uk/teaching/1314/NumMethods/supporting/mcmaster-kiruba-ludecomp.pdf
181 : * @param m, input 4x4 matrix
182 : * @param inv, Output inverted 4x4 matrix
183 : * @param n, dimension of square matrix
184 : * @returns false = matrix is Singular, true = matrix inversion successful
185 : */
186 : template<typename T>
187 940 : static bool mat_inverseN(const T* A, T* inv, uint16_t n)
188 : {
189 : T *L, *U, *P;
190 940 : bool ret = true;
191 940 : L = NEW_NOTHROW T[n*n];
192 940 : U = NEW_NOTHROW T[n*n];
193 940 : P = NEW_NOTHROW T[n*n];
194 940 : mat_LU_decompose(A,L,U,P,n);
195 :
196 940 : T *L_inv = NEW_NOTHROW T[n*n];
197 940 : T *U_inv = NEW_NOTHROW T[n*n];
198 :
199 940 : memset(L_inv,0,n*n*sizeof(T));
200 940 : mat_forward_sub(L,L_inv,n);
201 :
202 940 : memset(U_inv,0,n*n*sizeof(T));
203 940 : mat_back_sub(U,U_inv,n);
204 :
205 : // decomposed matrices no longer required
206 940 : delete[] L;
207 940 : delete[] U;
208 :
209 940 : T *inv_unpivoted = matrix_multiply(U_inv,L_inv,n);
210 940 : T *inv_pivoted = matrix_multiply(inv_unpivoted, P, n);
211 :
212 : //check sanity of results
213 9100 : for(uint16_t i = 0; i < n; i++) {
214 79800 : for(uint16_t j = 0; j < n; j++) {
215 71640 : if(isnan(inv_pivoted[i*n+j]) || isinf(inv_pivoted[i*n+j])){
216 0 : ret = false;
217 : }
218 : }
219 : }
220 940 : memcpy(inv,inv_pivoted,n*n*sizeof(T));
221 :
222 : //free memory
223 940 : delete[] inv_pivoted;
224 940 : delete[] inv_unpivoted;
225 940 : delete[] P;
226 940 : delete[] U_inv;
227 940 : delete[] L_inv;
228 940 : return ret;
229 : }
230 :
231 : /*
232 : * fast matrix inverse code only for 3x3 square matrix
233 : *
234 : * @param m, input 4x4 matrix
235 : * @param invOut, Output inverted 4x4 matrix
236 : * @returns false = matrix is Singular, true = matrix inversion successful
237 : */
238 : template<typename T>
239 0 : static bool inverse3x3(const T m[], T invOut[])
240 : {
241 : T inv[9];
242 : // computes the inverse of a matrix m
243 0 : T det = m[0] * (m[4] * m[8] - m[7] * m[5]) -
244 0 : m[1] * (m[3] * m[8] - m[5] * m[6]) +
245 0 : m[2] * (m[3] * m[7] - m[4] * m[6]);
246 0 : if (is_zero(det) || isinf(det)) {
247 0 : return false;
248 : }
249 :
250 0 : T invdet = 1 / det;
251 :
252 0 : inv[0] = (m[4] * m[8] - m[7] * m[5]) * invdet;
253 0 : inv[1] = (m[2] * m[7] - m[1] * m[8]) * invdet;
254 0 : inv[2] = (m[1] * m[5] - m[2] * m[4]) * invdet;
255 0 : inv[3] = (m[5] * m[6] - m[3] * m[8]) * invdet;
256 0 : inv[4] = (m[0] * m[8] - m[2] * m[6]) * invdet;
257 0 : inv[5] = (m[3] * m[2] - m[0] * m[5]) * invdet;
258 0 : inv[6] = (m[3] * m[7] - m[6] * m[4]) * invdet;
259 0 : inv[7] = (m[6] * m[1] - m[0] * m[7]) * invdet;
260 0 : inv[8] = (m[0] * m[4] - m[3] * m[1]) * invdet;
261 :
262 0 : for(uint16_t i = 0; i < 9; i++){
263 0 : invOut[i] = inv[i];
264 : }
265 :
266 0 : return true;
267 : }
268 :
269 : /*
270 : * fast matrix inverse code only for 4x4 square matrix copied from
271 : * gluInvertMatrix implementation in opengl for 4x4 matrices.
272 : *
273 : * @param m, input 4x4 matrix
274 : * @param invOut, Output inverted 4x4 matrix
275 : * @returns false = matrix is Singular, true = matrix inversion successful
276 : */
277 : template<typename T>
278 1033 : static bool inverse4x4(const T m[],T invOut[])
279 : {
280 : T inv[16], det;
281 : uint16_t i;
282 :
283 : #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
284 : //disable FE_INEXACT detection as it fails on mac os runs
285 1033 : int old = fedisableexcept(FE_INEXACT | FE_OVERFLOW);
286 : if (old < 0) {
287 : // hal.console->printf("inverse4x4(): warning: error on disabling FE_OVERFLOW floating point exception\n");
288 : }
289 : #endif
290 :
291 1033 : inv[0] = m[5] * m[10] * m[15] -
292 1033 : m[5] * m[11] * m[14] -
293 1033 : m[9] * m[6] * m[15] +
294 1033 : m[9] * m[7] * m[14] +
295 1033 : m[13] * m[6] * m[11] -
296 1033 : m[13] * m[7] * m[10];
297 :
298 1033 : inv[4] = -m[4] * m[10] * m[15] +
299 1033 : m[4] * m[11] * m[14] +
300 1033 : m[8] * m[6] * m[15] -
301 1033 : m[8] * m[7] * m[14] -
302 1033 : m[12] * m[6] * m[11] +
303 1033 : m[12] * m[7] * m[10];
304 :
305 1033 : inv[8] = m[4] * m[9] * m[15] -
306 1033 : m[4] * m[11] * m[13] -
307 1033 : m[8] * m[5] * m[15] +
308 1033 : m[8] * m[7] * m[13] +
309 1033 : m[12] * m[5] * m[11] -
310 1033 : m[12] * m[7] * m[9];
311 :
312 1033 : inv[12] = -m[4] * m[9] * m[14] +
313 1033 : m[4] * m[10] * m[13] +
314 1033 : m[8] * m[5] * m[14] -
315 1033 : m[8] * m[6] * m[13] -
316 1033 : m[12] * m[5] * m[10] +
317 1033 : m[12] * m[6] * m[9];
318 :
319 1033 : inv[1] = -m[1] * m[10] * m[15] +
320 1033 : m[1] * m[11] * m[14] +
321 1033 : m[9] * m[2] * m[15] -
322 1033 : m[9] * m[3] * m[14] -
323 1033 : m[13] * m[2] * m[11] +
324 1033 : m[13] * m[3] * m[10];
325 :
326 1033 : inv[5] = m[0] * m[10] * m[15] -
327 1033 : m[0] * m[11] * m[14] -
328 1033 : m[8] * m[2] * m[15] +
329 1033 : m[8] * m[3] * m[14] +
330 1033 : m[12] * m[2] * m[11] -
331 1033 : m[12] * m[3] * m[10];
332 :
333 1033 : inv[9] = -m[0] * m[9] * m[15] +
334 1033 : m[0] * m[11] * m[13] +
335 1033 : m[8] * m[1] * m[15] -
336 1033 : m[8] * m[3] * m[13] -
337 1033 : m[12] * m[1] * m[11] +
338 1033 : m[12] * m[3] * m[9];
339 :
340 1033 : inv[13] = m[0] * m[9] * m[14] -
341 1033 : m[0] * m[10] * m[13] -
342 1033 : m[8] * m[1] * m[14] +
343 1033 : m[8] * m[2] * m[13] +
344 1033 : m[12] * m[1] * m[10] -
345 1033 : m[12] * m[2] * m[9];
346 :
347 1033 : inv[2] = m[1] * m[6] * m[15] -
348 1033 : m[1] * m[7] * m[14] -
349 1033 : m[5] * m[2] * m[15] +
350 1033 : m[5] * m[3] * m[14] +
351 1033 : m[13] * m[2] * m[7] -
352 1033 : m[13] * m[3] * m[6];
353 :
354 1033 : inv[6] = -m[0] * m[6] * m[15] +
355 1033 : m[0] * m[7] * m[14] +
356 1033 : m[4] * m[2] * m[15] -
357 1033 : m[4] * m[3] * m[14] -
358 1033 : m[12] * m[2] * m[7] +
359 1033 : m[12] * m[3] * m[6];
360 :
361 1033 : inv[10] = m[0] * m[5] * m[15] -
362 1033 : m[0] * m[7] * m[13] -
363 1033 : m[4] * m[1] * m[15] +
364 1033 : m[4] * m[3] * m[13] +
365 1033 : m[12] * m[1] * m[7] -
366 1033 : m[12] * m[3] * m[5];
367 :
368 1033 : inv[14] = -m[0] * m[5] * m[14] +
369 1033 : m[0] * m[6] * m[13] +
370 1033 : m[4] * m[1] * m[14] -
371 1033 : m[4] * m[2] * m[13] -
372 1033 : m[12] * m[1] * m[6] +
373 1033 : m[12] * m[2] * m[5];
374 :
375 1033 : inv[3] = -m[1] * m[6] * m[11] +
376 1033 : m[1] * m[7] * m[10] +
377 1033 : m[5] * m[2] * m[11] -
378 1033 : m[5] * m[3] * m[10] -
379 1033 : m[9] * m[2] * m[7] +
380 1033 : m[9] * m[3] * m[6];
381 :
382 1033 : inv[7] = m[0] * m[6] * m[11] -
383 1033 : m[0] * m[7] * m[10] -
384 1033 : m[4] * m[2] * m[11] +
385 1033 : m[4] * m[3] * m[10] +
386 1033 : m[8] * m[2] * m[7] -
387 1033 : m[8] * m[3] * m[6];
388 :
389 1033 : inv[11] = -m[0] * m[5] * m[11] +
390 1033 : m[0] * m[7] * m[9] +
391 1033 : m[4] * m[1] * m[11] -
392 1033 : m[4] * m[3] * m[9] -
393 1033 : m[8] * m[1] * m[7] +
394 1033 : m[8] * m[3] * m[5];
395 :
396 1033 : inv[15] = m[0] * m[5] * m[10] -
397 1033 : m[0] * m[6] * m[9] -
398 1033 : m[4] * m[1] * m[10] +
399 1033 : m[4] * m[2] * m[9] +
400 1033 : m[8] * m[1] * m[6] -
401 1033 : m[8] * m[2] * m[5];
402 :
403 1033 : det = m[0] * inv[0] + m[1] * inv[4] + m[2] * inv[8] + m[3] * inv[12];
404 :
405 1033 : if (is_zero(det) || isinf(det)){
406 65 : return false;
407 : }
408 :
409 968 : det = 1.0f / det;
410 :
411 16456 : for (i = 0; i < 16; i++)
412 15488 : invOut[i] = inv[i] * det;
413 :
414 : #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
415 968 : if (old >= 0 && feenableexcept(old) < 0) {
416 : // hal.console->printf("inverse4x4(): warning: error on restoring floating exception mask\n");
417 : }
418 : #endif
419 :
420 968 : return true;
421 : }
422 :
423 : /*
424 : * generic matrix inverse code
425 : *
426 : * @param x, input nxn matrix
427 : * @param y, Output inverted nxn matrix
428 : * @param n, dimension of square matrix
429 : * @returns false = matrix is Singular, true = matrix inversion successful
430 : */
431 : template<typename T>
432 1973 : bool mat_inverse(const T x[], T y[], uint16_t dim)
433 : {
434 1973 : switch(dim){
435 0 : case 3: return inverse3x3(x,y);
436 1033 : case 4: return inverse4x4(x,y);
437 940 : default: return mat_inverseN(x,y,dim);
438 : }
439 : }
440 :
441 : template <typename T>
442 0 : void mat_mul(const T *A, const T *B, T *C, uint16_t n)
443 : {
444 0 : memset(C, 0, sizeof(T)*n*n);
445 0 : for(uint16_t i = 0; i < n; i++) {
446 0 : for(uint16_t j = 0; j < n; j++) {
447 0 : for(uint16_t k = 0;k < n; k++) {
448 0 : C[i*n + j] += A[i*n + k] * B[k*n + j];
449 : }
450 : }
451 : }
452 0 : }
453 :
454 : template <typename T>
455 0 : void mat_identity(T *A, uint16_t n)
456 : {
457 0 : memset(A, 0, sizeof(T)*n*n);
458 0 : for (uint16_t i=0; i<n; i++) {
459 0 : A[i*n+i] = 1;
460 : }
461 0 : }
462 :
463 : template bool mat_inverse<float>(const float x[], float y[], uint16_t dim);
464 : template void mat_mul<float>(const float *A, const float *B, float *C, uint16_t n);
465 : template void mat_identity<float>(float x[], uint16_t dim);
466 :
467 : template bool mat_inverse<double>(const double x[], double y[], uint16_t dim);
468 : template void mat_mul<double>(const double *A, const double *B, double *C, uint16_t n);
469 : template void mat_identity<double>(double x[], uint16_t dim);
|
