GCC Code Coverage Report

Directory: ./
File: Tools/math_utilities.hpp
Date: 2024-04-14 07:32:34
Exec Total Coverage
Lines: 279 308 90.6%
Branches: 182 342 53.2%
Line Branch Exec Source
1 // ______ ______ _ _ _____ ______
2 // | ____| ____| | (_)/ ____| | ____|
3 // | |__ | |__ | | _| (___ ___| |__
4 // | __| | __| | | | |\___ \ / __| __|
5 // | | | |____| |____| |____) | (__| |____
6 // |_| |______|______|_|_____/ \___|______|
7 // Finite Elements for Life Sciences and Engineering
8 //
9 // License: LGL2.1 License
10 // FELiScE default license: LICENSE in root folder
11 //
12 // Main authors: Vicente Mataix Ferrandiz
13 //
14
15 #ifndef _MATH_UTILITIES_HPP
16 #define _MATH_UTILITIES_HPP
17
18 // System includes
19 #include <vector>
20
21 // External includes
22
23 // Project includes
24 #include "Core/felisce.hpp"
25
26 namespace felisce
27 {
28 class MathUtilities
29 {
30 public:
31 /// The machine precision
32 static constexpr double ZeroTolerance = std::numeric_limits<double>::epsilon();
33
34 /**
35 * @brief This method checks the condition number of amtrix
36 * @param rInputMatrix Is the input matrix (unchanged at output)
37 * @param rInvertedMatrix Is the inverse of the input matrix
38 * @param Tolerance The maximum tolerance considered
39 */
40 template<class TMatrix1, class TMatrix2>
41 static inline bool CheckConditionNumber(
42 const TMatrix1& rInputMatrix,
43 TMatrix2& rInvertedMatrix,
44 const double Tolerance = ZeroTolerance,
45 const bool ThrowError = true
46 )
47 {
48 // We want at least 4 significant digits
49 const double max_condition_number = (1.0/Tolerance) * 1.0e-4;
50
51 // Find the condition number to define is inverse is OK
52 const double input_matrix_norm = norm_frobenius(rInputMatrix);
53 const double inverted_matrix_norm = norm_frobenius(rInvertedMatrix);
54
55 // Now the condition number is the product of both norms
56 const double cond_number = input_matrix_norm * inverted_matrix_norm ;
57 // Finally check if the condition number is low enough
58 if (cond_number > max_condition_number) {
59 if (ThrowError) {
60 FELISCE_WATCH(rInputMatrix);
61 FELISCE_WATCH(cond_number);
62 FEL_ERROR("Condition number of the matrix is too high!");
63 }
64 return false;
65 }
66
67 return true;
68 }
69
70 /**
71 * @brief This function is designed to be called when a dense linear system is needed to be solved
72 * @param A System matrix
73 * @param rX Solution vector. it's also the initial guess for iterative linear solvers.
74 * @param rB Right hand side vector.
75 */
76 1 static inline void Solve(
77 UBlasMatrix& rA,
78 UBlasVector& rX,
79 const UBlasVector& rB
80 )
81 {
82 1 const std::size_t size1 = rA.size1();
83
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 rX = rB;
84
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 UBlasPermutationMatrix pm(size1);
85
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 const int singular = lu_factorize(rA,pm);
86
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 1 times.
 1 if(singular == 1) {
87 ✗ FELISCE_WATCH(rA)
88 ✗ FEL_ERROR("Matrix is singular");
89 }
90
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 lu_substitute(rA, pm, rX);
91 1 }
92
93 /**
94 * @brief This function is designed to be called when a dense linear system is needed to be solved (copies to avoid changing A)
95 * @param rA System matrix
96 * @param rX Solution vector. it's also the initial guess for iterative linear solvers.
97 * @param rB Right hand side vector.
98 */
99 static inline void SolveWithCopy(
100 const UBlasMatrix& rA,
101 UBlasVector& rX,
102 const UBlasVector& rB
103 )
104 {
105 UBlasMatrix copy_A(rA);
106 Solve(copy_A, rX, rB);
107 }
108
109 /**
110 * @brief It inverts matrices of order 2
111 * @param rInputMatrix Is the input matrix (unchanged at output)
112 * @param rInvertedMatrix Is the inverse of the input matrix
113 * @param rInputMatrixDet Is the determinant of the input matrix
114 */
115 template<class TMatrix1, class TMatrix2>
116 25127520 static void InvertMatrix2(
117 const TMatrix1& rInputMatrix,
118 TMatrix2& rInvertedMatrix,
119 double& rInputMatrixDet
120 )
121 {
122
5/6
✓ Branch 1 taken 25127515 times.
✓ Branch 2 taken 2 times.
✗ Branch 4 not taken.
✓ Branch 5 taken 25127515 times.
✓ Branch 6 taken 2 times.
✓ Branch 7 taken 25127515 times.
 25127520 if(rInvertedMatrix.size1() != 2 || rInvertedMatrix.size2() != 2) {
123 4 rInvertedMatrix.resize(2,2,false);
124 }
125
126 25127520 rInputMatrixDet = rInputMatrix(0,0)*rInputMatrix(1,1)-rInputMatrix(0,1)*rInputMatrix(1,0);
127
128 25127520 rInvertedMatrix(0,0) = rInputMatrix(1,1);
129 25127520 rInvertedMatrix(0,1) = -rInputMatrix(0,1);
130 25127520 rInvertedMatrix(1,0) = -rInputMatrix(1,0);
131 25127520 rInvertedMatrix(1,1) = rInputMatrix(0,0);
132
133 25127520 rInvertedMatrix/=rInputMatrixDet;
134 25127520 }
135
136 /**
137 * @brief It inverts matrices of order 3
138 * @param rInputMatrix Is the input matrix (unchanged at output)
139 * @param rInvertedMatrix Is the inverse of the input matrix
140 * @param rInputMatrixDet Is the determinant of the input matrix
141 */
142 template<class TMatrix1, class TMatrix2>
143 182244321 static void InvertMatrix3(
144 const TMatrix1& rInputMatrix,
145 TMatrix2& rInvertedMatrix,
146 double& rInputMatrixDet
147 )
148 {
149
3/6
✓ Branch 1 taken 182244320 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 182244320 times.
✗ Branch 6 not taken.
✓ Branch 7 taken 182244320 times.
 182244321 if(rInvertedMatrix.size1() != 3 || rInvertedMatrix.size2() != 3) {
150 ✗ rInvertedMatrix.resize(3,3,false);
151 }
152
153 // Filling the inverted matrix with the algebraic complements
154 // First column
155 182244321 rInvertedMatrix(0,0) = rInputMatrix(1,1)*rInputMatrix(2,2) - rInputMatrix(1,2)*rInputMatrix(2,1);
156 182244321 rInvertedMatrix(1,0) = -rInputMatrix(1,0)*rInputMatrix(2,2) + rInputMatrix(1,2)*rInputMatrix(2,0);
157 182244321 rInvertedMatrix(2,0) = rInputMatrix(1,0)*rInputMatrix(2,1) - rInputMatrix(1,1)*rInputMatrix(2,0);
158
159 // Second column
160 182244321 rInvertedMatrix(0,1) = -rInputMatrix(0,1)*rInputMatrix(2,2) + rInputMatrix(0,2)*rInputMatrix(2,1);
161 182244321 rInvertedMatrix(1,1) = rInputMatrix(0,0)*rInputMatrix(2,2) - rInputMatrix(0,2)*rInputMatrix(2,0);
162 182244321 rInvertedMatrix(2,1) = -rInputMatrix(0,0)*rInputMatrix(2,1) + rInputMatrix(0,1)*rInputMatrix(2,0);
163
164 // Third column
165 182244321 rInvertedMatrix(0,2) = rInputMatrix(0,1)*rInputMatrix(1,2) - rInputMatrix(0,2)*rInputMatrix(1,1);
166 182244321 rInvertedMatrix(1,2) = -rInputMatrix(0,0)*rInputMatrix(1,2) + rInputMatrix(0,2)*rInputMatrix(1,0);
167 182244321 rInvertedMatrix(2,2) = rInputMatrix(0,0)*rInputMatrix(1,1) - rInputMatrix(0,1)*rInputMatrix(1,0);
168
169 // Calculation of determinant (of the input matrix)
170 182244321 rInputMatrixDet = rInputMatrix(0,0)*rInvertedMatrix(0,0) + rInputMatrix(0,1)*rInvertedMatrix(1,0) + rInputMatrix(0,2)*rInvertedMatrix(2,0);
171
172 // Finalizing the calculation of the inverted matrix
173 182244321 rInvertedMatrix /= rInputMatrixDet;
174 182244321 }
175
176 /**
177 * @brief It inverts matrices of order 4
178 * @param rInputMatrix Is the input matrix (unchanged at output)
179 * @param rInvertedMatrix Is the inverse of the input matrix
180 * @param rInputMatrixDet Is the determinant of the input matrix
181 */
182 template<class TMatrix1, class TMatrix2>
183 4 static void InvertMatrix4(
184 const TMatrix1& rInputMatrix,
185 TMatrix2& rInvertedMatrix,
186 double& rInputMatrixDet
187 )
188 {
189
3/6
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
✓ Branch 7 taken 2 times.
 4 if (rInvertedMatrix.size1() != 4 || rInvertedMatrix.size2() != 4) {
190 ✗ rInvertedMatrix.resize(4, 4, false);
191 }
192
193 /* Compute inverse of the Matrix */
194 // First column
195 4 rInvertedMatrix(0, 0) = -(rInputMatrix(1, 3) * rInputMatrix(2, 2) * rInputMatrix(3, 1)) + rInputMatrix(1, 2) * rInputMatrix(2, 3) * rInputMatrix(3, 1) + rInputMatrix(1, 3) * rInputMatrix(2, 1) * rInputMatrix(3, 2) - rInputMatrix(1, 1) * rInputMatrix(2, 3) * rInputMatrix(3, 2) - rInputMatrix(1, 2) * rInputMatrix(2, 1) * rInputMatrix(3, 3) + rInputMatrix(1, 1) * rInputMatrix(2, 2) * rInputMatrix(3, 3);
196 4 rInvertedMatrix(0, 1) = rInputMatrix(0, 3) * rInputMatrix(2, 2) * rInputMatrix(3, 1) - rInputMatrix(0, 2) * rInputMatrix(2, 3) * rInputMatrix(3, 1) - rInputMatrix(0, 3) * rInputMatrix(2, 1) * rInputMatrix(3, 2) + rInputMatrix(0, 1) * rInputMatrix(2, 3) * rInputMatrix(3, 2) + rInputMatrix(0, 2) * rInputMatrix(2, 1) * rInputMatrix(3, 3) - rInputMatrix(0, 1) * rInputMatrix(2, 2) * rInputMatrix(3, 3);
197 4 rInvertedMatrix(0, 2) = -(rInputMatrix(0, 3) * rInputMatrix(1, 2) * rInputMatrix(3, 1)) + rInputMatrix(0, 2) * rInputMatrix(1, 3) * rInputMatrix(3, 1) + rInputMatrix(0, 3) * rInputMatrix(1, 1) * rInputMatrix(3, 2) - rInputMatrix(0, 1) * rInputMatrix(1, 3) * rInputMatrix(3, 2) - rInputMatrix(0, 2) * rInputMatrix(1, 1) * rInputMatrix(3, 3) + rInputMatrix(0, 1) * rInputMatrix(1, 2) * rInputMatrix(3, 3);
198 4 rInvertedMatrix(0, 3) = rInputMatrix(0, 3) * rInputMatrix(1, 2) * rInputMatrix(2, 1) - rInputMatrix(0, 2) * rInputMatrix(1, 3) * rInputMatrix(2, 1) - rInputMatrix(0, 3) * rInputMatrix(1, 1) * rInputMatrix(2, 2) + rInputMatrix(0, 1) * rInputMatrix(1, 3) * rInputMatrix(2, 2) + rInputMatrix(0, 2) * rInputMatrix(1, 1) * rInputMatrix(2, 3) - rInputMatrix(0, 1) * rInputMatrix(1, 2) * rInputMatrix(2, 3);
199
200 // Second column
201 4 rInvertedMatrix(1, 0) = rInputMatrix(1, 3) * rInputMatrix(2, 2) * rInputMatrix(3, 0) - rInputMatrix(1, 2) * rInputMatrix(2, 3) * rInputMatrix(3, 0) - rInputMatrix(1, 3) * rInputMatrix(2, 0) * rInputMatrix(3, 2) + rInputMatrix(1, 0) * rInputMatrix(2, 3) * rInputMatrix(3, 2) + rInputMatrix(1, 2) * rInputMatrix(2, 0) * rInputMatrix(3, 3) - rInputMatrix(1, 0) * rInputMatrix(2, 2) * rInputMatrix(3, 3);
202 4 rInvertedMatrix(1, 1) = -(rInputMatrix(0, 3) * rInputMatrix(2, 2) * rInputMatrix(3, 0)) + rInputMatrix(0, 2) * rInputMatrix(2, 3) * rInputMatrix(3, 0) + rInputMatrix(0, 3) * rInputMatrix(2, 0) * rInputMatrix(3, 2) - rInputMatrix(0, 0) * rInputMatrix(2, 3) * rInputMatrix(3, 2) - rInputMatrix(0, 2) * rInputMatrix(2, 0) * rInputMatrix(3, 3) + rInputMatrix(0, 0) * rInputMatrix(2, 2) * rInputMatrix(3, 3);
203 4 rInvertedMatrix(1, 2) = rInputMatrix(0, 3) * rInputMatrix(1, 2) * rInputMatrix(3, 0) - rInputMatrix(0, 2) * rInputMatrix(1, 3) * rInputMatrix(3, 0) - rInputMatrix(0, 3) * rInputMatrix(1, 0) * rInputMatrix(3, 2) + rInputMatrix(0, 0) * rInputMatrix(1, 3) * rInputMatrix(3, 2) + rInputMatrix(0, 2) * rInputMatrix(1, 0) * rInputMatrix(3, 3) - rInputMatrix(0, 0) * rInputMatrix(1, 2) * rInputMatrix(3, 3);
204 4 rInvertedMatrix(1, 3) = -(rInputMatrix(0, 3) * rInputMatrix(1, 2) * rInputMatrix(2, 0)) + rInputMatrix(0, 2) * rInputMatrix(1, 3) * rInputMatrix(2, 0) + rInputMatrix(0, 3) * rInputMatrix(1, 0) * rInputMatrix(2, 2) - rInputMatrix(0, 0) * rInputMatrix(1, 3) * rInputMatrix(2, 2) - rInputMatrix(0, 2) * rInputMatrix(1, 0) * rInputMatrix(2, 3) + rInputMatrix(0, 0) * rInputMatrix(1, 2) * rInputMatrix(2, 3);
205
206 // Third column
207 4 rInvertedMatrix(2, 0) = -(rInputMatrix(1, 3) * rInputMatrix(2, 1) * rInputMatrix(3, 0)) + rInputMatrix(1, 1) * rInputMatrix(2, 3) * rInputMatrix(3, 0) + rInputMatrix(1, 3) * rInputMatrix(2, 0) * rInputMatrix(3, 1) - rInputMatrix(1, 0) * rInputMatrix(2, 3) * rInputMatrix(3, 1) - rInputMatrix(1, 1) * rInputMatrix(2, 0) * rInputMatrix(3, 3) + rInputMatrix(1, 0) * rInputMatrix(2, 1) * rInputMatrix(3, 3);
208 4 rInvertedMatrix(2, 1) = rInputMatrix(0, 3) * rInputMatrix(2, 1) * rInputMatrix(3, 0) - rInputMatrix(0, 1) * rInputMatrix(2, 3) * rInputMatrix(3, 0) - rInputMatrix(0, 3) * rInputMatrix(2, 0) * rInputMatrix(3, 1) + rInputMatrix(0, 0) * rInputMatrix(2, 3) * rInputMatrix(3, 1) + rInputMatrix(0, 1) * rInputMatrix(2, 0) * rInputMatrix(3, 3) - rInputMatrix(0, 0) * rInputMatrix(2, 1) * rInputMatrix(3, 3);
209 4 rInvertedMatrix(2, 2) = -(rInputMatrix(0, 3) * rInputMatrix(1, 1) * rInputMatrix(3, 0)) + rInputMatrix(0, 1) * rInputMatrix(1, 3) * rInputMatrix(3, 0) + rInputMatrix(0, 3) * rInputMatrix(1, 0) * rInputMatrix(3, 1) - rInputMatrix(0, 0) * rInputMatrix(1, 3) * rInputMatrix(3, 1) - rInputMatrix(0, 1) * rInputMatrix(1, 0) * rInputMatrix(3, 3) + rInputMatrix(0, 0) * rInputMatrix(1, 1) * rInputMatrix(3, 3);
210 4 rInvertedMatrix(2, 3) = rInputMatrix(0, 3) * rInputMatrix(1, 1) * rInputMatrix(2, 0) - rInputMatrix(0, 1) * rInputMatrix(1, 3) * rInputMatrix(2, 0) - rInputMatrix(0, 3) * rInputMatrix(1, 0) * rInputMatrix(2, 1) + rInputMatrix(0, 0) * rInputMatrix(1, 3) * rInputMatrix(2, 1) + rInputMatrix(0, 1) * rInputMatrix(1, 0) * rInputMatrix(2, 3) - rInputMatrix(0, 0) * rInputMatrix(1, 1) * rInputMatrix(2, 3);
211
212 // Fourth column
213 4 rInvertedMatrix(3, 0) = rInputMatrix(1, 2) * rInputMatrix(2, 1) * rInputMatrix(3, 0) - rInputMatrix(1, 1) * rInputMatrix(2, 2) * rInputMatrix(3, 0) - rInputMatrix(1, 2) * rInputMatrix(2, 0) * rInputMatrix(3, 1) + rInputMatrix(1, 0) * rInputMatrix(2, 2) * rInputMatrix(3, 1) + rInputMatrix(1, 1) * rInputMatrix(2, 0) * rInputMatrix(3, 2) - rInputMatrix(1, 0) * rInputMatrix(2, 1) * rInputMatrix(3, 2);
214 4 rInvertedMatrix(3, 1) = -(rInputMatrix(0, 2) * rInputMatrix(2, 1) * rInputMatrix(3, 0)) + rInputMatrix(0, 1) * rInputMatrix(2, 2) * rInputMatrix(3, 0) + rInputMatrix(0, 2) * rInputMatrix(2, 0) * rInputMatrix(3, 1) - rInputMatrix(0, 0) * rInputMatrix(2, 2) * rInputMatrix(3, 1) - rInputMatrix(0, 1) * rInputMatrix(2, 0) * rInputMatrix(3, 2) + rInputMatrix(0, 0) * rInputMatrix(2, 1) * rInputMatrix(3, 2);
215 4 rInvertedMatrix(3, 2) = rInputMatrix(0, 2) * rInputMatrix(1, 1) * rInputMatrix(3, 0) - rInputMatrix(0, 1) * rInputMatrix(1, 2) * rInputMatrix(3, 0) - rInputMatrix(0, 2) * rInputMatrix(1, 0) * rInputMatrix(3, 1) + rInputMatrix(0, 0) * rInputMatrix(1, 2) * rInputMatrix(3, 1) + rInputMatrix(0, 1) * rInputMatrix(1, 0) * rInputMatrix(3, 2) - rInputMatrix(0, 0) * rInputMatrix(1, 1) * rInputMatrix(3, 2);
216 4 rInvertedMatrix(3, 3) = -(rInputMatrix(0, 2) * rInputMatrix(1, 1) * rInputMatrix(2, 0)) + rInputMatrix(0, 1) * rInputMatrix(1, 2) * rInputMatrix(2, 0) + rInputMatrix(0, 2) * rInputMatrix(1, 0) * rInputMatrix(2, 1) - rInputMatrix(0, 0) * rInputMatrix(1, 2) * rInputMatrix(2, 1) - rInputMatrix(0, 1) * rInputMatrix(1, 0) * rInputMatrix(2, 2) + rInputMatrix(0, 0) * rInputMatrix(1, 1) * rInputMatrix(2, 2);
217
218 // Calculation of determinant (of the input matrix)
219 4 rInputMatrixDet = rInputMatrix(0, 1) * rInputMatrix(1, 3) * rInputMatrix(2, 2) * rInputMatrix(3, 0) - rInputMatrix(0, 1) * rInputMatrix(1, 2) * rInputMatrix(2, 3) * rInputMatrix(3, 0) - rInputMatrix(0, 0) * rInputMatrix(1, 3) * rInputMatrix(2, 2) * rInputMatrix(3, 1) + rInputMatrix(0, 0) * rInputMatrix(1, 2) * rInputMatrix(2, 3) * rInputMatrix(3, 1) - rInputMatrix(0, 1) * rInputMatrix(1, 3) * rInputMatrix(2, 0) * rInputMatrix(3, 2) + rInputMatrix(0, 0) * rInputMatrix(1, 3) * rInputMatrix(2, 1) * rInputMatrix(3, 2) + rInputMatrix(0, 1) * rInputMatrix(1, 0) * rInputMatrix(2, 3) * rInputMatrix(3, 2) - rInputMatrix(0, 0) * rInputMatrix(1, 1) * rInputMatrix(2, 3) * rInputMatrix(3, 2) + rInputMatrix(0, 3) * (rInputMatrix(1, 2) * rInputMatrix(2, 1) * rInputMatrix(3, 0) - rInputMatrix(1, 1) * rInputMatrix(2, 2) * rInputMatrix(3, 0) - rInputMatrix(1, 2) * rInputMatrix(2, 0) * rInputMatrix(3, 1) + rInputMatrix(1, 0) * rInputMatrix(2, 2) * rInputMatrix(3, 1) + rInputMatrix(1, 1) * rInputMatrix(2, 0) * rInputMatrix(3, 2) - rInputMatrix(1, 0) * rInputMatrix(2, 1) * rInputMatrix(3, 2)) + (rInputMatrix(0, 1) * rInputMatrix(1, 2) * rInputMatrix(2, 0) - rInputMatrix(0, 0) * rInputMatrix(1, 2) * rInputMatrix(2, 1) - rInputMatrix(0, 1) * rInputMatrix(1, 0) * rInputMatrix(2, 2) + rInputMatrix(0, 0) * rInputMatrix(1, 1) * rInputMatrix(2, 2)) * rInputMatrix(3, 3) + rInputMatrix(0, 2) * (-(rInputMatrix(1, 3) * rInputMatrix(2, 1) * rInputMatrix(3, 0)) + rInputMatrix(1, 1) * rInputMatrix(2, 3) * rInputMatrix(3, 0) + rInputMatrix(1, 3) * rInputMatrix(2, 0) * rInputMatrix(3, 1) - rInputMatrix(1, 0) * rInputMatrix(2, 3) * rInputMatrix(3, 1) - rInputMatrix(1, 1) * rInputMatrix(2, 0) * rInputMatrix(3, 3) + rInputMatrix(1, 0) * rInputMatrix(2, 1) * rInputMatrix(3, 3));
220
221 // Finalizing the calculation of the inverted matrix
222 4 rInvertedMatrix /= rInputMatrixDet;
223 }
224
225 /**
226 * @brief It inverts matrices of order 2, 3 and 4
227 * @param rInputMatrix Is the input matrix (unchanged at output)
228 * @param rInvertedMatrix Is the inverse of the input matrix
229 * @param rInputMatrixDet Is the determinant of the input matrix
230 */
231 template<class TMatrix1, class TMatrix2>
232 207377371 static void InvertMatrix(
233 const TMatrix1& rInputMatrix,
234 TMatrix2& rInvertedMatrix,
235 double& rInputMatrixDet,
236 const double Tolerance = ZeroTolerance
237 )
238 {
239 (void) Tolerance;
240 207377371 const std::size_t size = rInputMatrix.size2();
241
242
2/2
✓ Branch 0 taken 5521 times.
✓ Branch 1 taken 207371841 times.
 207377371 if(size == 1) {
243
3/6
✓ Branch 1 taken 5521 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 5521 times.
✗ Branch 6 not taken.
✓ Branch 7 taken 5521 times.
 5522 if(rInvertedMatrix.size1() != 1 || rInvertedMatrix.size2() != 1) {
244 ✗ rInvertedMatrix.resize(1,1,false);
245 }
246 5522 rInvertedMatrix(0,0) = 1.0/rInputMatrix(0,0);
247 5522 rInputMatrixDet = rInputMatrix(0,0);
248
2/2
✓ Branch 0 taken 25127517 times.
✓ Branch 1 taken 182244324 times.
 207371849 } else if (size == 2) {
249 25127520 InvertMatrix2(rInputMatrix, rInvertedMatrix, rInputMatrixDet);
250
2/2
✓ Branch 0 taken 182244320 times.
✓ Branch 1 taken 4 times.
 182244329 } else if (size == 3) {
251 182244321 InvertMatrix3(rInputMatrix, rInvertedMatrix, rInputMatrixDet);
252
2/2
✓ Branch 0 taken 2 times.
✓ Branch 1 taken 2 times.
 8 } else if (size == 4) {
253 4 InvertMatrix4(rInputMatrix, rInvertedMatrix, rInputMatrixDet);
254 } else if (std::is_same<TMatrix1, UBlasMatrix>::value) {
255
256 2 const std::size_t size1 = rInputMatrix.size1();
257 2 const std::size_t size2 = rInputMatrix.size2();
258
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 7 taken 1 times.
 2 if(rInvertedMatrix.size1() != size1 || rInvertedMatrix.size2() != size2) {
259 ✗ rInvertedMatrix.resize(size1, size2,false);
260 }
261
262
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 UBlasMatrix A(rInputMatrix);
263
264
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 2 UBlasPermutationMatrix pm(A.size1());
265
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 const int singular = lu_factorize(A,pm);
266
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 2 rInvertedMatrix.assign( UBlasIdentityMatrix(A.size1()));
267
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 1 times.
 2 if(singular == 1) {
268 ✗ std::cout << "Matrix is singular: " << rInputMatrix << std::endl;
269 ✗ exit(1);
270 }
271
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 lu_substitute(A, pm, rInvertedMatrix);
272
273 // Calculating determinant
274 2 rInputMatrixDet = 1.0;
275
276
2/2
✓ Branch 0 taken 5 times.
✓ Branch 1 taken 1 times.
 12 for (std::size_t i = 0; i < size1; ++i) {
277
1/2
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
 10 std::size_t ki = pm[i] == i ? 0 : 1;
278
2/6
✓ Branch 0 taken 5 times.
✗ Branch 1 not taken.
✓ Branch 3 taken 5 times.
✗ Branch 4 not taken.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
 10 rInputMatrixDet *= (ki == 0) ? A(i,i) : -A(i,i);
279 }
280 2 } else { // Bounded-matrix case
281 2 const std::size_t size1 = rInputMatrix.size1();
282 2 const std::size_t size2 = rInputMatrix.size2();
283
284
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 UBlasMatrix A(rInputMatrix);
285
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 UBlasMatrix invA(rInvertedMatrix);
286
287
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 UBlasPermutationMatrix pm(size1);
288
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 const int singular = lu_factorize(A,pm);
289
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 invA.assign( UBlasIdentityMatrix(size1));
290
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 1 times.
 2 if(singular == 1) {
291 ✗ std::cout << "Matrix is singular: " << rInputMatrix << std::endl;
292 ✗ exit(1);
293 }
294
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 lu_substitute(A, pm, invA);
295
296 // Calculating determinant
297 2 rInputMatrixDet = 1.0;
298
299
2/2
✓ Branch 0 taken 5 times.
✓ Branch 1 taken 1 times.
 12 for (std::size_t i = 0; i < size1;++i) {
300
1/2
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
 10 std::size_t ki = pm[i] == i ? 0 : 1;
301
2/6
✓ Branch 0 taken 5 times.
✗ Branch 1 not taken.
✓ Branch 3 taken 5 times.
✗ Branch 4 not taken.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
 10 rInputMatrixDet *= (ki == 0) ? A(i,i) : -A(i,i);
302 }
303
304
2/2
✓ Branch 0 taken 5 times.
✓ Branch 1 taken 1 times.
 12 for (std::size_t i = 0; i < size1;++i) {
305
2/2
✓ Branch 0 taken 25 times.
✓ Branch 1 taken 5 times.
 60 for (std::size_t j = 0; j < size2;++j) {
306
2/4
✓ Branch 1 taken 25 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 25 times.
✗ Branch 5 not taken.
 50 rInvertedMatrix(i,j) = invA(i,j);
307 }
308 }
309 }
310
311 // Checking condition number
312 // if (Tolerance > 0.0) { // Check is skipped for negative tolerances
313 // CheckConditionNumber(rInputMatrix, rInvertedMatrix, Tolerance);
314 // }
315 207377371 }
316
317 /**
318 * @brief It inverts non square matrices (https://en.wikipedia.org/wiki/Inverse_element#Matrices)
319 * @param rInputMatrix Is the input matrix (unchanged at output)
320 * @param rInvertedMatrix Is the inverse of the input matrix
321 * @param rInputMatrixDet Is the determinant of the input matrix
322 */
323 template<class TMatrixType>
324 2 static void GeneralizedInvertMatrix(
325 const TMatrixType& rInputMatrix,
326 TMatrixType& rInvertedMatrix,
327 double& rInputMatrixDet
328 )
329 {
330 2 const std::size_t size_1 = rInputMatrix.size1();
331 2 const std::size_t size_2 = rInputMatrix.size2();
332
333
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 2 times.
 2 if (size_1 == size_2) {
334 ✗ InvertMatrix(rInputMatrix, rInvertedMatrix, rInputMatrixDet);
335
2/2
✓ Branch 0 taken 1 times.
✓ Branch 1 taken 1 times.
 2 } else if (size_1 < size_2) { // Right inverse
336
2/6
✗ Branch 1 not taken.
✓ Branch 2 taken 1 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
 1 if (rInvertedMatrix.size1() != size_2 || rInvertedMatrix.size2() != size_1) {
337
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 rInvertedMatrix.resize(size_2, size_1, false);
338 }
339
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
 2 const TMatrixType aux = prod(rInputMatrix, trans(rInputMatrix));
340
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 TMatrixType auxInv;
341
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 InvertMatrix(aux, auxInv, rInputMatrixDet);
342 1 rInputMatrixDet = std::sqrt(rInputMatrixDet);
343
4/8
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
 1 noalias(rInvertedMatrix) = prod(trans(rInputMatrix), auxInv);
344 1 } else { // Left inverse
345
2/6
✗ Branch 1 not taken.
✓ Branch 2 taken 1 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
 1 if (rInvertedMatrix.size1() != size_2 || rInvertedMatrix.size2() != size_1) {
346
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 rInvertedMatrix.resize(size_2, size_1, false);
347 }
348
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
 2 const TMatrixType aux = prod(trans(rInputMatrix), rInputMatrix);
349
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 TMatrixType auxInv;
350
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 InvertMatrix(aux, auxInv, rInputMatrixDet);
351 1 rInputMatrixDet = std::sqrt(rInputMatrixDet);
352
4/8
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
 1 noalias(rInvertedMatrix) = prod(auxInv, trans(rInputMatrix));
353 1 }
354 2 }
355
356 /**
357 * @brief Calculates the determinant of a matrix of dimension 2x2 (no check performed on matrix size)
358 * @param rA Is the input matrix
359 * @return The determinant of the 2x2 matrix
360 */
361 template<class TMatrixType>
362 59146 static inline double Det2(const TMatrixType& rA)
363 {
364 59146 return (rA(0,0)*rA(1,1)-rA(0,1)*rA(1,0));
365 }
366
367 /**
368 * @brief Calculates the determinant of a matrix of dimension 3*3 (no check performed on matrix size)
369 * @param rA Is the input matrix
370 * @return The determinant of the 3x3 matrix
371 */
372 template<class TMatrixType>
373 663002 static inline double Det3(const TMatrixType& rA)
374 {
375 // Calculating the algebraic complements to the first line
376 663002 const double a = rA(1,1)*rA(2,2) - rA(1,2)*rA(2,1);
377 663002 const double b = rA(1,0)*rA(2,2) - rA(1,2)*rA(2,0);
378 663002 const double c = rA(1,0)*rA(2,1) - rA(1,1)*rA(2,0);
379
380 663002 return rA(0,0)*a - rA(0,1)*b + rA(0,2)*c;
381 }
382
383 /**
384 * @brief Calculates the determinant of a matrix of dimension 4*4 (no check performed on matrix size)
385 * @param rA Is the input matrix
386 * @return The determinant of the 4x4 matrix
387 */
388 template<class TMatrixType>
389 2 static inline double Det4(const TMatrixType& rA)
390 {
391 2 const double Det = rA(0,1)*rA(1,3)*rA(2,2)*rA(3,0)-rA(0,1)*rA(1,2)*rA(2,3)*rA(3,0)-rA(0,0)*rA(1,3)*rA(2,2)*rA(3,1)+rA(0,0)*rA(1,2)*rA(2,3)*rA(3,1)
392 2 -rA(0,1)*rA(1,3)*rA(2,0)*rA(3,2)+rA(0,0)*rA(1,3)*rA(2,1)*rA(3,2)+rA(0,1)*rA(1,0)*rA(2,3)*rA(3,2)-rA(0,0)*rA(1,1)*rA(2,3)*rA(3,2)+rA(0,3)*(rA(1,2)*rA(2,1)*rA(3,0)-rA(1,1)*rA(2,2)*rA(3,0)-rA(1,2)*rA(2,0)*rA(3,1)+rA(1,0)*rA(2,2)*rA(3,1)+rA(1,1)*rA(2,0)*rA(3,2)
393 2 -rA(1,0)*rA(2,1)*rA(3,2))+(rA(0,1)*rA(1,2)*rA(2,0)-rA(0,0)*rA(1,2)*rA(2,1)-rA(0,1)*rA(1,0)*rA(2,2)+rA(0,0)*rA(1,1)*rA(2,2))*rA(3,3)+rA(0,2)*(-(rA(1,3)*rA(2,1)*rA(3,0))+rA(1,1)*rA(2,3)*rA(3,0)+rA(1,3)*rA(2,0)*rA(3,1)-rA(1,0)*rA(2,3)*rA(3,1)-rA(1,1)*rA(2,0)*rA(3,3)+rA(1,0)*rA(2,1)*rA(3,3));
394 2 return Det;
395 }
396
397 /**
398 * @brief Calculates the determinant of a matrix of dimension 2x2 or 3x3 (no check performed on matrix size)
399 * @param rA Is the input matrix
400 * @return The determinant of the 2x2 matrix
401 */
402 template<class TMatrixType>
403 722165 static inline double Det(const TMatrixType& rA)
404 {
405
4/4
✓ Branch 1 taken 59144 times.
✓ Branch 2 taken 663001 times.
✓ Branch 3 taken 1 times.
✓ Branch 4 taken 13 times.
 722165 switch ( rA.size1() )
406 {
407 59146 case 2:
408
1/2
✓ Branch 1 taken 59144 times.
✗ Branch 2 not taken.
 59146 return Det2(rA);
409 break;
410 663002 case 3:
411
1/2
✓ Branch 1 taken 663001 times.
✗ Branch 2 not taken.
 663002 return Det3(rA);
412 break;
413 2 case 4:
414
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 return Det4(rA);
415 15 default:
416
1/2
✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
 15 UBlasMatrix Aux(rA);
417
1/2
✓ Branch 2 taken 13 times.
✗ Branch 3 not taken.
 15 UBlasPermutationMatrix pm(Aux.size1());
418
1/2
✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
 15 bool singular = lu_factorize(Aux,pm);
419
420
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 13 times.
 15 if (singular) {
421 ✗ return 0.0;
422 }
423
424 15 double det = 1.0;
425
426
2/2
✓ Branch 1 taken 17 times.
✓ Branch 2 taken 13 times.
 38 for (std::size_t i = 0; i < Aux.size1();++i) {
427
1/2
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
 23 std::size_t ki = pm[i] == i ? 0 : 1;
428
1/2
✓ Branch 2 taken 17 times.
✗ Branch 3 not taken.
 23 det *= std::pow(-1.0, ki) * Aux(i,i);
429 }
430
431 15 return det;
432 15 }
433 }
434
435 /**
436 * @brief Calculates the determinant of a matrix of dimension 2x2 or 3x3 (no check performed on matrix size)
437 * @param rA Is the input matrix
438 * @return The determinant of the 2x2 matrix
439 */
440 template<class TMatrixType>
441 1 static inline double GeneralizedDet(const TMatrixType& rA)
442 {
443 double determinant;
444
445
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 1 times.
 1 if (rA.size1() == rA.size2()) {
446 ✗ determinant = Det(rA);
447
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 } else if (rA.size1() < rA.size2()) { // Right determinant
448
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
 2 const UBlasMatrix AAT = prod( rA, trans(rA) );
449
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 determinant = std::sqrt(Det(AAT));
450 1 } else { // Left determinant
451 ✗ const UBlasMatrix ATA = prod( trans(rA), rA );
452 ✗ determinant = std::sqrt(Det(ATA));
453 }
454
455 1 return determinant;
456 }
457
458 /**
459 * @brief Calculates the product operation B'DB
460 * @param rA The resulting matrix
461 * @param rD The "center" matrix
462 * @param rB The matrices to be transposed
463 * @tparam TMatrixType1 The type of matrix considered (1)
464 * @tparam TMatrixType2 The type of matrix considered (2)
465 * @tparam TMatrixType3 The type of matrix considered (3)
466 */
467 template<class TMatrixType1, class TMatrixType2, class TMatrixType3>
468 579304 static inline void BtDBProductOperation(
469 TMatrixType1& rA,
470 const TMatrixType2& rD,
471 const TMatrixType3& rB
472 )
473 {
474 // The sizes
475 579304 const std::size_t size1 = rB.size2();
476 579304 const std::size_t size2 = rB.size2();
477
478
3/6
✓ Branch 1 taken 579304 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 579304 times.
✗ Branch 6 not taken.
✓ Branch 7 taken 579304 times.
 579304 if (rA.size1() != size1 || rA.size2() != size2)
479 ✗ rA.resize(size1, size2, false);
480
481 // Direct multiplication
482 // noalias(rA) = prod( trans( rB ), UBlasMatrix(prod(rD, rB)));
483
484 // Manual multiplication
485 579304 rA.clear();
486
2/2
✓ Branch 1 taken 3475821 times.
✓ Branch 2 taken 579304 times.
 4055125 for(std::size_t k = 0; k< rD.size1(); ++k) {
487
2/2
✓ Branch 1 taken 20854917 times.
✓ Branch 2 taken 3475821 times.
 24330738 for(std::size_t l = 0; l < rD.size2(); ++l) {
488 20854917 const double Dkl = rD(k, l);
489
2/2
✓ Branch 1 taken 563082543 times.
✓ Branch 2 taken 20854917 times.
 583937460 for(std::size_t j = 0; j < rB.size2(); ++j) {
490 563082543 const double DklBlj = Dkl * rB(l, j);
491
2/2
✓ Branch 1 taken 15203228013 times.
✓ Branch 2 taken 563082543 times.
 15766310556 for(std::size_t i = 0; i< rB.size2(); ++i) {
492 15203228013 rA(i, j) += rB(k, i) * DklBlj;
493 }
494 }
495 }
496 }
497 579304 }
498
499 /**
500 * @brief Calculates the product operation BDB'
501 * @param rA The resulting matrix
502 * @param rD The "center" matrix
503 * @param rB The matrices to be transposed
504 * @tparam TMatrixType1 The type of matrix considered (1)
505 * @tparam TMatrixType2 The type of matrix considered (2)
506 * @tparam TMatrixType3 The type of matrix considered (3)
507 */
508 template<class TMatrixType1, class TMatrixType2, class TMatrixType3>
509 144491 static inline void BDBtProductOperation(
510 TMatrixType1& rA,
511 const TMatrixType2& rD,
512 const TMatrixType3& rB
513 )
514 {
515 // The sizes
516 144491 const std::size_t size1 = rB.size1();
517 144491 const std::size_t size2 = rB.size1();
518
519
3/6
✓ Branch 1 taken 72246 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 72246 times.
✗ Branch 6 not taken.
✓ Branch 7 taken 72246 times.
 144491 if (rA.size1() != size1 || rA.size2() != size2)
520 ✗ rA.resize(size1, size2, false);
521
522 // Direct multiplication
523 // noalias(rA) = prod(rB, UBlasMatrix(prod(rD, trans(rB))));
524
525 // Manual multiplication
526 144491 rA.clear();
527
2/2
✓ Branch 1 taken 866943 times.
✓ Branch 2 taken 72246 times.
 1878374 for(std::size_t k = 0; k< rD.size1(); ++k) {
528
2/2
✓ Branch 1 taken 10403289 times.
✓ Branch 2 taken 866943 times.
 22540452 for(std::size_t l = 0; l < rD.size2(); ++l) {
529 20806569 const double Dkl = rD(k,l);
530
2/2
✓ Branch 1 taken 124839387 times.
✓ Branch 2 taken 10403289 times.
 270485316 for(std::size_t j = 0; j < rB.size1(); ++j) {
531 249678747 const double DklBjl = Dkl * rB(j,l);
532
2/2
✓ Branch 1 taken 1498072401 times.
✓ Branch 2 taken 124839387 times.
 3245823468 for(std::size_t i = 0; i< rB.size1(); ++i) {
533 2996144721 rA(i, j) += rB(i, k) * DklBjl;
534 }
535 }
536 }
537 }
538 144491 }
539
540 /**
541 * @brief Performs the vector product of the two input vectors a,b
542 * @details a,b are assumed to be of size 3 (no check is performed on vector sizes)
543 * @param a First input vector
544 * @param b Second input vector
545 * @return The resulting vector
546 */
547 template<class T>
548 224288 static inline T CrossProduct(
549 const T& a,
550 const T& b
551 )
552 {
553 224288 T c(a);
554
555 224288 c[0] = a[1]*b[2] - a[2]*b[1];
556 224288 c[1] = a[2]*b[0] - a[0]*b[2];
557 224288 c[2] = a[0]*b[1] - a[1]*b[0];
558
559 224288 return c;
560 }
561
562 /**
563 * @brief Performs the cross product of the two input vectors a,b
564 * @details a,b are assumed to be of size 3 (check is only performed on vector sizes in debug mode)
565 * @param a First input vector
566 * @param b Second input vector
567 * @param c The resulting vector
568 */
569 template< class T1, class T2 , class T3>
570 107429 static inline void CrossProduct(T1& c, const T2& a, const T3& b ){
571 // if (c.size() != 3) c.resize(3);
572
573
4/8
✓ Branch 1 taken 71774 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 71774 times.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✓ Branch 8 taken 71774 times.
✗ Branch 9 not taken.
✓ Branch 10 taken 71774 times.
 107429 FEL_ASSERT(a.size() == 3 && b.size() == 3 && c.size() == 3);
574 // if (a.size() != 3 || b.size() != 3 || c.size() != 3)
575 // FEL_ERROR("The size of the vectors is different of 3");
576
577 107429 c[0] = a[1]*b[2] - a[2]*b[1];
578 107429 c[1] = a[2]*b[0] - a[0]*b[2];
579 107429 c[2] = a[0]*b[1] - a[1]*b[0];
580 107429 }
581
582 /**
583 * @brief Performs the unitary cross product of the two input vectors a,b
584 * @details a,b are assumed to be of size 3 (no check is performed on vector sizes)
585 * @param a First input vector
586 * @param b Second input vector
587 * @param c The resulting vector
588 */
589 template< class T1, class T2 , class T3>
590 1149 static inline void UnitCrossProduct(T1& c, const T2& a, const T3& b ){
591
1/2
✓ Branch 1 taken 1149 times.
✗ Branch 2 not taken.
 1149 CrossProduct(c,a,b);
592
1/2
✓ Branch 1 taken 1149 times.
✗ Branch 2 not taken.
 1149 const double norm = norm_2(c);
593
1/4
✗ Branch 0 not taken.
✓ Branch 1 taken 1149 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
 1149 FEL_ASSERT(norm > 1000.0*ZeroTolerance);
594
1/2
✓ Branch 1 taken 1149 times.
✗ Branch 2 not taken.
 1149 c/=norm;
595 1149 }
596
597 /**
598 * @brief This computes a orthonormal basis from a given vector (Frisvad method)
599 * @param c The input vector
600 * @param a First resulting vector
601 * @param b Second resulting vector
602 * @param Type The type of method employed, 0 is HughesMoeller, 1 is Frisvad and otherwise Naive
603 */
604 template< class T1, class T2 , class T3>
605 1122 static inline void OrthonormalBasis(const T1& c,T2& a,T3& b, const std::size_t Type = 0 ){
606
1/2
✓ Branch 0 taken 1122 times.
✗ Branch 1 not taken.
 1122 if (Type == 0) {
607 1122 OrthonormalBasisHughesMoeller(c,a,b);
608 ✗ } else if (Type == 1) {
609 ✗ OrthonormalBasisFrisvad(c,a,b);
610 } else {
611 ✗ OrthonormalBasisNaive(c,a,b);
612 }
613 1122 }
614
615 /**
616 * @brief This computes a orthonormal basis from a given vector (Hughes Moeller method)
617 * @param c The input vector
618 * @param a First resulting vector
619 * @param b Second resulting vector
620 * @note Orthonormal basis taken from: http://orbit.dtu.dk/files/126824972/onb_frisvad_jgt2012_v2.pdf
621 */
622 template< class T1, class T2 , class T3>
623 1123 static inline void OrthonormalBasisHughesMoeller(const T1& c,T2& a,T3& b){
624
3/6
✓ Branch 1 taken 1123 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 1123 times.
✗ Branch 6 not taken.
✓ Branch 7 taken 1123 times.
 1123 FEL_ASSERT(!(norm_2(c) < (1.0 - 1.0e-6) || norm_2(c) > (1.0 + 1.0e-6)));
625 // Choose a vector orthogonal to n as the direction of b2.
626
2/2
✓ Branch 4 taken 662 times.
✓ Branch 5 taken 461 times.
 1123 if(std::abs(c[0]) > std::abs(c[2])) {
627 662 b[0] = c[1];
628 662 b[1] = -c[0];
629 662 b[2] = 0.0;
630 } else {
631 461 b[0] = 0.0;
632 461 b[1] = c[2];
633 461 b[2] = -c[1];
634 }
635
1/2
✓ Branch 2 taken 1123 times.
✗ Branch 3 not taken.
 1123 b /= norm_2(b); // Normalize b
636 1123 UnitCrossProduct(a, b , c); // Construct a using a cross product
637 1123 }
638
639 /**
640 * @brief This computes a orthonormal basis from a given vector (Frisvad method)
641 * @param c The input vector
642 * @param a First resulting vector
643 * @param b Second resulting vector
644 * @note Orthonormal basis taken from: http://orbit.dtu.dk/files/126824972/onb_frisvad_jgt2012_v2.pdf
645 */
646 template< class T1, class T2 , class T3>
647 1 static inline void OrthonormalBasisFrisvad(const T1& c,T2& a,T3& b){
648
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 7 taken 1 times.
 1 FEL_ASSERT(!(norm_2(c) < (1.0 - 1.0e-3) || norm_2(c) > (1.0 + 1.0e-3)));
649
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 if ((c[2] + 1.0) > 1.0e4 * ZeroTolerance) {
650 1 a[0] = 1.0 - std::pow(c[0], 2)/(1.0 + c[2]);
651 1 a[1] = - (c[0] * c[1])/(1.0 + c[2]);
652 1 a[2] = - c[0];
653
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 const double norm_a = norm_2(a);
654
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 a /= norm_a;
655 1 b[0] = - (c[0] * c[1])/(1.0 + c[2]);
656 1 b[1] = 1.0 - std::pow(c[1], 2)/(1.0 + c[2]);
657 1 b[2] = -c[1];
658
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 const double norm_b = norm_2(b);
659
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 b /= norm_b;
660 } else { // In case that the vector is in negative Z direction
661 ✗ a[0] = 1.0;
662 ✗ a[1] = 0.0;
663 ✗ a[2] = 0.0;
664 ✗ b[0] = 0.0;
665 ✗ b[1] = -1.0;
666 ✗ b[2] = 0.0;
667 }
668 1 }
669
670 /**
671 * @brief This computes a orthonormal basis from a given vector (Naive method)
672 * @param c The input vector
673 * @param a First resulting vector
674 * @param b Second resulting vector
675 * @note Orthonormal basis taken from: http://orbit.dtu.dk/files/126824972/onb_frisvad_jgt2012_v2.pdf
676 */
677 template< class T1, class T2 , class T3>
678 1 static inline void OrthonormalBasisNaive(const T1& c,T2& a,T3& b){
679
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 7 taken 1 times.
 1 FEL_ASSERT(!(norm_2(c) < (1.0 - 1.0e-3) || norm_2(c) > (1.0 + 1.0e-3)));
680 // If c is near the x-axis , use the y-axis. Otherwise use the x-axis.
681
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 1 times.
 1 if(c[0] > 0.9f) {
682 ✗ a[0] = 0.0;
683 ✗ a[1] = 1.0;
684 ✗ a[2] = 0.0;
685 } else {
686 1 a[0] = 1.0;
687 1 a[1] = 0.0;
688 1 a[2] = 0.0;
689 }
690
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 1 a -= c * inner_prod(a, c); // Make a orthogonal to c
691
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 a /= norm_2(a); // Normalize a
692 1 UnitCrossProduct(b, c, a); // Construct b using a cross product
693 1 }
694
695 /**
696 * @brief Computes the Euler angles relative to the given axis of coordinates
697 * @param a First vector coordinates
698 * @param b Second vector coordinates
699 * @param c Third vector coordinates
700 * @param pre precession rotation
701 * @param nut nutation rotation
702 * @param rot intrinsic rotation
703 * @tparam T1 The type of first vector coordinates
704 * @tparam T2 The type of second vector coordinates
705 * @tparam T3 The type of third vector coordinates
706 */
707 template< class T1, class T2 , class T3>
708 18583 static inline void LCS2Euler(const T1& a, const T2& b, const T3& c, double& pre, double& nut, double& rot)
709 {
710 18583 const double Z1xy = std::sqrt(std::pow(c[0], 2) + std::pow(c[1], 2));
711
2/2
✓ Branch 1 taken 8461 times.
✓ Branch 2 taken 10122 times.
 18583 if (Z1xy > std::numeric_limits<double>::epsilon()) {
712 8461 pre = std::atan2(b[0] * c[1] - b[1] * c[0], a[0] * c[1] - a[1] * c[0]);
713 8461 nut = std::atan2(Z1xy, c[2]);
714 8461 rot = -std::atan2(-c[0], c[1]);
715 } else {
716 10122 pre = 0.0;
717
1/2
✓ Branch 1 taken 10122 times.
✗ Branch 2 not taken.
 10122 nut = (c[2] > 0.0) ? 0.0 : M_PI;
718 10122 rot = -std::atan2(a[1], a[0]);
719 }
720 18583 }
721
722 /**
723 * @brief Computes the rotation matrix
724 * @param rR The rotation matrix
725 * @param pre precession rotation
726 * @param nut nutation rotation
727 * @param rot intrinsic rotation
728 * @tparam T1 The type of rotation matrix
729 */
730 template< class T1>
731 18583 static inline void Euler2RotationMatrix(T1& rR, const double pre, const double nut, const double rot)
732 {
733 18583 rR(0,0) = std::cos(rot) * std::cos(nut);
734 18583 rR(0,1) = (std::cos(rot) * std::sin(nut) * std::sin(pre) - std::sin(rot) * std::cos(pre));
735 18583 rR(0,2) = (std::cos(rot) * std::sin(nut) * std::cos(pre) + std::sin(rot) * std::sin(pre));
736 18583 rR(1,0) = std::sin(rot) * std::cos(nut);
737 18583 rR(1,1) = (std::sin(rot) * std::sin(nut) * std::sin(pre) + std::cos(rot) * std::cos(pre));
738 18583 rR(1,2) = (std::sin(rot) * std::sin(nut) * std::cos(pre) - std::cos(rot) * std::sin(pre));
739 18583 rR(2,0) = -std::sin(nut);
740 18583 rR(2,1) = std::cos(nut) * std::sin(pre);
741 18583 rR(2,2) = std::cos(nut) * std::cos(pre);
742 18583 }
743
744 /**
745 * @brief This matrix extends the local matrix to all the dofs matrix
746 * @param rLocalMatrix The local matrix
747 * @param rAllDofsMatrix The global matrix
748 * @tparam T1 The type of local matrix
749 * @tparam T2 The type of all dofs matrix
750 */
751 template<class T1, class T2>
752 1 static inline void ExtendToAllDofsLocalMatrix(
753 const T1& rLocalMatrix,
754 T2& rAllDofsMatrix
755 )
756 {
757 1 rAllDofsMatrix.clear();
758
2/2
✓ Branch 2 taken 4 times.
✓ Branch 3 taken 1 times.
 5 for (std::size_t kk = 0; kk < rAllDofsMatrix.size1(); kk += rLocalMatrix.size1()) {
759
2/2
✓ Branch 1 taken 12 times.
✓ Branch 2 taken 4 times.
 16 for (std::size_t i = 0; i < rLocalMatrix.size1(); ++i) {
760
2/2
✓ Branch 1 taken 36 times.
✓ Branch 2 taken 12 times.
 48 for (std::size_t j = 0; j < rLocalMatrix.size2(); ++j) {
761
2/2
✓ Branch 2 taken 12 times.
✓ Branch 3 taken 24 times.
 36 if (std::abs(rLocalMatrix(i, j)) > ZeroTolerance) {
762 12 rAllDofsMatrix(i + kk, j + kk) = rLocalMatrix(i, j);
763 }
764 }
765 }
766 }
767 1 }
768
769 /**
770 * @brief This matrix extends the local matrix to all the dofs matrix
771 * @details Following FELiScE ordering
772 * @param rLocalMatrix The local matrix
773 * @param rAllDofsMatrix The global matrix
774 * @tparam T1 The type of local matrix
775 * @tparam T2 The type of all dofs matrix
776 */
777 template<class T1, class T2>
778 53551 static inline void ExtendToAllDofsLocalMatrixFELiScEOrdering(
779 const T1& rLocalMatrix,
780 T2& rAllDofsMatrix,
781 const std::size_t NumberNodes
782 )
783 {
784 // Auxiliary value
785 53551 const std::size_t number_dofs_types = rAllDofsMatrix.size1() / (rLocalMatrix.size1() * NumberNodes);
786 53551 const std::size_t delta_increment_dof = rAllDofsMatrix.size1() / NumberNodes;
787
788 // Clear
789 53551 rAllDofsMatrix.clear();
790
791 // Iterate and assign
792
2/2
✓ Branch 0 taken 107102 times.
✓ Branch 1 taken 53551 times.
 160653 for (std::size_t jj = 0; jj < number_dofs_types; ++jj) {
793
2/2
✓ Branch 0 taken 214204 times.
✓ Branch 1 taken 107102 times.
 321306 for (std::size_t kk = 0; kk < NumberNodes; ++kk) {
794
2/2
✓ Branch 1 taken 642612 times.
✓ Branch 2 taken 214204 times.
 856816 for (std::size_t i = 0; i < rLocalMatrix.size1(); ++i) {
795
2/2
✓ Branch 1 taken 1927836 times.
✓ Branch 2 taken 642612 times.
 2570448 for (std::size_t j = 0; j < rLocalMatrix.size2(); ++j) {
796
2/2
✓ Branch 2 taken 1363180 times.
✓ Branch 3 taken 564656 times.
 1927836 if (std::abs(rLocalMatrix(i, j)) > ZeroTolerance) {
797 1363180 rAllDofsMatrix(NumberNodes * i + kk + jj * delta_increment_dof, NumberNodes * j + kk + jj * delta_increment_dof) = rLocalMatrix(i, j);
798 }
799 }
800 }
801 }
802 }
803 53551 }
804
805
806 };
807
808 }
809
810 #endif
811