blob: cc658bde3d1dffa4142f336da02e225038496131 (
plain)
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
|
/* matrix.c - matrix-algebra code
*
* This file is Copyright (c)2014-2018 by the GPSD project
* SPDX-License-Identifier: BSD-2-clause
*/
#include <math.h>
#include <stdbool.h>
#include "matrix.h"
bool matrix_invert(double mat[4][4], double inverse[4][4])
/* selected elements from 4x4 matrox inversion */
{
// Find all NECESSARY 2x2 subdeterminants
double Det2_12_01 = mat[1][0] * mat[2][1] - mat[1][1] * mat[2][0];
double Det2_12_02 = mat[1][0] * mat[2][2] - mat[1][2] * mat[2][0];
//double Det2_12_03 = mat[1][0]*mat[2][3] - mat[1][3]*mat[2][0];
double Det2_12_12 = mat[1][1] * mat[2][2] - mat[1][2] * mat[2][1];
//double Det2_12_13 = mat[1][1]*mat[2][3] - mat[1][3]*mat[2][1];
//double Det2_12_23 = mat[1][2]*mat[2][3] - mat[1][3]*mat[2][2];
double Det2_13_01 = mat[1][0] * mat[3][1] - mat[1][1] * mat[3][0];
//double Det2_13_02 = mat[1][0]*mat[3][2] - mat[1][2]*mat[3][0];
double Det2_13_03 = mat[1][0] * mat[3][3] - mat[1][3] * mat[3][0];
//double Det2_13_12 = mat[1][1]*mat[3][2] - mat[1][2]*mat[3][1];
double Det2_13_13 = mat[1][1] * mat[3][3] - mat[1][3] * mat[3][1];
//double Det2_13_23 = mat[1][2]*mat[3][3] - mat[1][3]*mat[3][2];
double Det2_23_01 = mat[2][0] * mat[3][1] - mat[2][1] * mat[3][0];
double Det2_23_02 = mat[2][0] * mat[3][2] - mat[2][2] * mat[3][0];
double Det2_23_03 = mat[2][0] * mat[3][3] - mat[2][3] * mat[3][0];
double Det2_23_12 = mat[2][1] * mat[3][2] - mat[2][2] * mat[3][1];
double Det2_23_13 = mat[2][1] * mat[3][3] - mat[2][3] * mat[3][1];
double Det2_23_23 = mat[2][2] * mat[3][3] - mat[2][3] * mat[3][2];
// Find all NECESSARY 3x3 subdeterminants
double Det3_012_012 = mat[0][0] * Det2_12_12 - mat[0][1] * Det2_12_02
+ mat[0][2] * Det2_12_01;
//double Det3_012_013 = mat[0][0]*Det2_12_13 - mat[0][1]*Det2_12_03
// + mat[0][3]*Det2_12_01;
//double Det3_012_023 = mat[0][0]*Det2_12_23 - mat[0][2]*Det2_12_03
// + mat[0][3]*Det2_12_02;
//double Det3_012_123 = mat[0][1]*Det2_12_23 - mat[0][2]*Det2_12_13
// + mat[0][3]*Det2_12_12;
//double Det3_013_012 = mat[0][0]*Det2_13_12 - mat[0][1]*Det2_13_02
// + mat[0][2]*Det2_13_01;
double Det3_013_013 = mat[0][0] * Det2_13_13 - mat[0][1] * Det2_13_03
+ mat[0][3] * Det2_13_01;
//double Det3_013_023 = mat[0][0]*Det2_13_23 - mat[0][2]*Det2_13_03
// + mat[0][3]*Det2_13_02;
//double Det3_013_123 = mat[0][1]*Det2_13_23 - mat[0][2]*Det2_13_13
// + mat[0][3]*Det2_13_12;
//double Det3_023_012 = mat[0][0]*Det2_23_12 - mat[0][1]*Det2_23_02
// + mat[0][2]*Det2_23_01;
//double Det3_023_013 = mat[0][0]*Det2_23_13 - mat[0][1]*Det2_23_03
// + mat[0][3]*Det2_23_01;
double Det3_023_023 = mat[0][0] * Det2_23_23 - mat[0][2] * Det2_23_03
+ mat[0][3] * Det2_23_02;
//double Det3_023_123 = mat[0][1]*Det2_23_23 - mat[0][2]*Det2_23_13
// + mat[0][3]*Det2_23_12;
double Det3_123_012 = mat[1][0] * Det2_23_12 - mat[1][1] * Det2_23_02
+ mat[1][2] * Det2_23_01;
double Det3_123_013 = mat[1][0] * Det2_23_13 - mat[1][1] * Det2_23_03
+ mat[1][3] * Det2_23_01;
double Det3_123_023 = mat[1][0] * Det2_23_23 - mat[1][2] * Det2_23_03
+ mat[1][3] * Det2_23_02;
double Det3_123_123 = mat[1][1] * Det2_23_23 - mat[1][2] * Det2_23_13
+ mat[1][3] * Det2_23_12;
// Find the 4x4 determinant
static double det;
det = mat[0][0] * Det3_123_123
- mat[0][1] * Det3_123_023
+ mat[0][2] * Det3_123_013 - mat[0][3] * Det3_123_012;
// Very small determinants probably reflect floating-point fuzz near zero
if (fabs(det) < 0.0001)
return false;
inverse[0][0] = Det3_123_123 / det;
//inverse[0][1] = -Det3_023_123 / det;
//inverse[0][2] = Det3_013_123 / det;
//inverse[0][3] = -Det3_012_123 / det;
//inverse[1][0] = -Det3_123_023 / det;
inverse[1][1] = Det3_023_023 / det;
//inverse[1][2] = -Det3_013_023 / det;
//inverse[1][3] = Det3_012_023 / det;
//inverse[2][0] = Det3_123_013 / det;
//inverse[2][1] = -Det3_023_013 / det;
inverse[2][2] = Det3_013_013 / det;
//inverse[2][3] = -Det3_012_013 / det;
//inverse[3][0] = -Det3_123_012 / det;
//inverse[3][1] = Det3_023_012 / det;
//inverse[3][2] = -Det3_013_012 / det;
inverse[3][3] = Det3_012_012 / det;
return true;
}
#ifdef __UNUSED_
// cppcheck-suppress unusedFunction
void matrix_symmetrize(double mat[4][4], double prod[4][4])
/* symmetrize a matrix, multiply it by its transpose */
{
int i, j, k;
for (i = 0; i < 4; ++i) { //< rows
for (j = 0; j < 4; ++j) { //< cols
prod[i][j] = 0.0;
for (k = 0; k < 4; ++k) {
prod[i][j] += mat[k][i] * mat[k][j];
}
}
}
}
#endif /* UNUSED */
/* end */
|
