Barebones Static CTR Library 2.1.4
Loading...
Searching...
No Matches
mathOperations.hpp
1#pragma once
2
3// #define _USE_MATH_DEFINES
4#include <cmath>
5#include <blaze/Math.h>
6#include <iostream>
7
8namespace mathOp
9{
13 enum class rootFindingMethod
14 {
15 NEWTON_RAPHSON,
16 LEVENBERG_MARQUARDT,
17 POWELL_DOG_LEG,
18 MODIFIED_NEWTON_RAPHSON,
19 BROYDEN,
20 BROYDEN_II
21 };
22
29 inline constexpr double deg2Rad(double degree)
30 {
31 constexpr double pi_180 = M_PI / 180.00;
32 return degree * pi_180;
33 }
34
41 inline double congruentAngle(double angle)
42 {
43 constexpr double THREE_PI = 3.00 * M_PI;
44 return std::fmod(std::fabs(angle), THREE_PI) * (angle < 0.00 ? -1.00 : 1.00);
45 }
46
53 inline blaze::StaticVector<double, 3UL> orthogonal(const blaze::StaticVector<double, 3UL> &v)
54 {
55 blaze::StaticVector<double, 3UL> aux;
56 if (std::fabs(v[0UL]) < std::fabs(v[1UL]))
57 {
58 aux[std::fabs(v[0UL]) < std::fabs(v[2UL]) ? 0UL : 2UL] = 1.00;
59 }
60 else
61 {
62 aux[std::fabs(v[1UL]) < std::fabs(v[2UL]) ? 1UL : 2UL] = 1.00;
63 }
64
65 return blaze::cross(v, aux);
66 }
67
75 inline blaze::StaticVector<double, 4UL> getRotationBetween(const blaze::StaticVector<double, 3UL> &a, const blaze::StaticVector<double, 3UL> &b)
76 {
77 blaze::StaticVector<double, 3UL> from = blaze::normalize(a);
78 blaze::StaticVector<double, 3UL> to = blaze::normalize(b);
79 blaze::StaticVector<double, 4UL> quaternion;
80
81 // Handles the case of parallel vectors & opposing directions
82 if (from == -to)
83 {
84 quaternion[blaze::argmax(from)] = 1.00;
85 }
86 else
87 {
88 double cos_theta = blaze::dot(from, to);
89 double half_cos = std::sqrt((1.00 + cos_theta) * 0.50);
90 blaze::StaticVector<double, 3UL> axis = blaze::cross(from, to) / (2.00 * half_cos);
91 quaternion = {half_cos, axis[0UL], axis[1UL], axis[2UL]};
92 }
93
94 return quaternion;
95 }
96
103 inline blaze::HybridMatrix<double, 6UL, 6UL> pInv(const blaze::HybridMatrix<double, 6UL, 6UL> &M)
104 {
105 // declaring the auxiliary matrices for pInv computation
106 blaze::HybridMatrix<double, 6UL, 6UL> U; // The matrix for the left singular vectors
107 blaze::HybridVector<double, 6UL> s; // The vector for the singular values
108 blaze::HybridMatrix<double, 6UL, 6UL> V; // The matrix for the right singular vectors
109
110 // SVD decomposition of the matrix M
111 try
112 {
113 blaze::svd(M, U, s, V);
114 }
115 catch (std::exception &e)
116 {
117 std::cerr << "Blaze SVD has failed: " << e.what() << std::endl;
118 std::cerr << M << std::endl;
119 }
120
121 // computing the pseudoinverse of M via SVD decomposition
122 blaze::DiagonalMatrix<blaze::HybridMatrix<double, 6UL, 6UL>> S_inv(s.size(), s.size());
123
124 // Creating a reference to the diagonal of matrix S
125 auto diag = blaze::diagonal(S_inv);
126 // applies a "damping factor" to the zero singular values of the matrix M
127 diag = blaze::map(s, [](double d)
128 { return (d <= 1.00E-25) ? 0.00 : d / ((d * d) + 1.00E-25); }); // Damped least squares -- SVD pseudo inverse
129
130 return blaze::trans(U * S_inv * V);
131 }
132
139 inline void SO3_To_Quaternion(blaze::StaticVector<double, 4UL> &h, const blaze::StaticMatrix<double, 3UL, 3UL> &R)
140 {
141 double n4; // the norm of quaternion multiplied by 4
142 const double tr = blaze::trace(R); // trace of matrix
143
144 if (tr > 0.00)
145 {
146 h[0UL] = tr + 1.00;
147 h[1UL] = R(1UL, 2UL) - R(2UL, 1UL);
148 h[2UL] = R(2UL, 0UL) - R(0UL, 2UL);
149 h[3UL] = R(0UL, 1UL) - R(1UL, 0UL);
150 n4 = h[0UL];
151 }
152 else
153 {
154 size_t i = 0UL;
155 if (R(1UL, 1UL) > R(0UL, 0UL))
156 i = 1UL;
157 if (R(2UL, 2UL) > R(i, i))
158 i = 2UL;
159
160 switch (i)
161 {
162 case 0UL:
163 h[0UL] = R(1UL, 2UL) - R(2UL, 1UL);
164 h[1UL] = 1.00 + R(0UL, 0UL) - R(1UL, 1UL) - R(2UL, 2UL);
165 h[2UL] = R(1UL, 0UL) + R(0UL, 1UL);
166 h[3UL] = R(2UL, 0UL) + R(0UL, 2UL);
167 n4 = h[1UL];
168 break;
169 case 1UL:
170 h[0UL] = R(2UL, 0UL) - R(0UL, 2UL);
171 h[1UL] = R(1UL, 0UL) + R(0UL, 1UL);
172 h[2UL] = 1.00 + R(1UL, 1UL) - R(0UL, 0UL) - R(2UL, 2UL);
173 h[3UL] = R(2UL, 1UL) + R(1UL, 2UL);
174 n4 = h[2UL];
175 break;
176 case 2UL:
177 h[0UL] = R(0UL, 1UL) - R(1UL, 0UL);
178 h[1UL] = R(2UL, 0UL) + R(0UL, 2UL);
179 h[2UL] = R(2UL, 1UL) + R(1UL, 2UL);
180 h[3UL] = 1.00 + R(2UL, 2UL) - R(0UL, 0UL) - R(1UL, 1UL);
181 n4 = h[3UL];
182 break;
183 }
184 }
185
186 h *= 1.00 / (2.00 * std::sqrt(n4));
187 }
188
198 inline void euler2Quaternion(const double heading, const double attitude, const double bank, blaze::StaticVector<double, 4UL> &h)
199 {
200 // Gotta convert the angles to radians first.
201 double theta(0.50 * heading), phi(0.50 * attitude), psi(0.50 * bank);
202 double c1(cos(theta)), s1(sin(theta)), c2(cos(phi)), s2(sin(phi)), c3(cos(psi)), s3(sin(psi)), c1c2, s1s2;
203
204 c1c2 = c1 * c2;
205 s1s2 = s1 * s2;
206
207 h[0UL] = c1c2 * c3 - s1s2 * s3;
208 h[1UL] = c1c2 * s3 + s1s2 * c3;
209 h[2UL] = s1 * c2 * c3 + c1 * s2 * s3;
210 h[3UL] = c1 * s2 * c3 - s1 * c2 * s3;
211 }
212
220 inline void getSO3(const blaze::StaticVector<double, 4UL> &h, blaze::StaticMatrix<double, 3UL, 3UL, blaze::columnMajor> &R)
221 {
222 const double scale = 2.00 / blaze::sqrNorm(h);
223
224 R(0UL, 0UL) = 1.00 + scale * (-h[2UL] * h[2UL] - h[3UL] * h[3UL]);
225 R(0UL, 1UL) = scale * (h[1UL] * h[2UL] - h[3UL] * h[0UL]);
226 R(0UL, 2UL) = scale * (h[1UL] * h[3UL] + h[2UL] * h[0UL]);
227
228 R(1UL, 0UL) = scale * (h[1UL] * h[2UL] + h[3UL] * h[0UL]);
229 R(1UL, 1UL) = 1.00 + scale * (-h[1UL] * h[1UL] - h[3UL] * h[3UL]);
230 R(1UL, 2UL) = scale * (h[2UL] * h[3UL] - h[1UL] * h[0UL]);
231
232 R(2UL, 0UL) = scale * (h[1UL] * h[3UL] - h[2UL] * h[0UL]);
233 R(2UL, 1UL) = scale * (h[2UL] * h[3UL] + h[1UL] * h[0UL]);
234 R(2UL, 2UL) = 1.00 + scale * (-h[1UL] * h[1UL] - h[2UL] * h[2UL]);
235 }
236
243 inline blaze::StaticMatrix<double, 3UL, 3UL> rotz(const double &theta)
244 {
245 double c(cos(theta)), s(sin(theta));
246 blaze::StaticMatrix<double, 3UL, 3UL> R{{c, -s, 0.00},
247 {s, c, 0.00},
248 {0.00, 0.00, 1.00}};
249
250 return R;
251 }
252
259 inline blaze::StaticMatrix<double, 3UL, 3UL> rotz_dot_transpose(const double &theta)
260 {
261 double c(cos(theta)), s(sin(theta));
262 blaze::StaticMatrix<double, 3UL, 3UL> dR{{-s, c, 0.00},
263 {-c, -s, 0.00},
264 {0.00, 0.00, 0.00}};
265
266 return dR;
267 }
268
275 inline blaze::StaticMatrix<double, 3UL, 3UL> hatOperator(const blaze::StaticVector<double, 3UL> &v)
276 {
277 blaze::StaticMatrix<double, 3UL, 3UL> Res{{0.00, -v[2UL], v[1UL]},
278 {v[2UL], 0.00, -v[0UL]},
279 {-v[1UL], v[0UL], 0.00}};
280
281 return Res;
282 }
283
292 inline blaze::StaticMatrix<double, 3UL, 3UL> hatPreMultiply(const blaze::StaticVector<double, 3UL> &v, const blaze::StaticMatrix<double, 3UL, 3UL> &M)
293 {
294 blaze::StaticMatrix<double, 3UL, 3UL> Res{{-M(1UL, 0UL) * v[2UL] + M(2UL, 0UL) * v[1UL], -M(1UL, 1UL) * v[2UL] + M(2UL, 1UL) * v[1UL], -M(1UL, 2UL) * v[2UL] + M(2UL, 2UL) * v[1UL]},
295 {M(0UL, 0UL) * v[2UL] - M(2UL, 0UL) * v[0UL], M(0UL, 1UL) * v[2UL] - M(2UL, 1UL) * v[0UL], M(0UL, 2UL) * v[2UL] - M(2UL, 2UL) * v[0UL]},
296 {-M(0UL, 0UL) * v[1UL] + M(1UL, 0UL) * v[0UL], -M(0UL, 1UL) * v[1UL] + M(1UL, 1UL) * v[0UL], -M(0UL, 2UL) * v[1UL] + M(1UL, 2UL) * v[0UL]}};
297
298 return Res;
299 }
300
309 inline blaze::StaticMatrix<double, 3UL, 3UL> hatPostMultiply(const blaze::StaticMatrix<double, 3UL, 3UL> &M, const blaze::StaticVector<double, 3UL> &v)
310 {
311 blaze::StaticMatrix<double, 3UL, 3UL> Res{{M(0UL, 1UL) * v[2UL] - M(0UL, 2UL) * v[1UL], -M(0UL, 0UL) * v[2UL] + M(0UL, 2UL) * v[0UL], M(0UL, 0UL) * v[1UL] - M(0UL, 1UL) * v[0UL]},
312 {M(1UL, 1UL) * v[2UL] - M(1UL, 2UL) * v[1UL], -M(1UL, 0UL) * v[2UL] + M(1UL, 2UL) * v[0UL], M(1UL, 0UL) * v[1UL] - M(1UL, 1UL) * v[0UL]},
313 {M(2UL, 1UL) * v[2UL] - M(2UL, 2UL) * v[1UL], -M(2UL, 0UL) * v[2UL] + M(2UL, 2UL) * v[0UL], M(2UL, 0UL) * v[1UL] - M(2UL, 1UL) * v[0UL]}};
314
315 return Res;
316 }
317
326 inline blaze::StaticVector<double, 3UL> transposePreMultiply(const blaze::StaticMatrix<double, 3UL, 3UL> &R, const blaze::StaticVector<double, 3UL> &v)
327 {
328 blaze::StaticVector<double, 3UL> res{R(0UL, 0UL) * v[0UL] + R(1UL, 0UL) * v[1UL] + R(2UL, 0UL) * v[2UL],
329 R(0UL, 1UL) * v[0UL] + R(1UL, 1UL) * v[1UL] + R(2UL, 1UL) * v[2UL],
330 R(0UL, 2UL) * v[0UL] + R(1UL, 2UL) * v[1UL] + R(2UL, 2UL) * v[2UL]};
331
332 return res;
333 }
334
342 inline blaze::StaticVector<double, 4UL> quaternionDiff(const blaze::StaticVector<double, 3UL> &u, const blaze::StaticVector<double, 4UL> &h)
343 {
344 blaze::StaticVector<double, 4UL> hs{0.50 * (-u[0UL] * h[1UL] - u[1UL] * h[2UL] - u[2UL] * h[3UL]),
345 0.50 * (u[0UL] * h[0UL] + u[2UL] * h[2UL] - u[1UL] * h[3UL]),
346 0.50 * (u[1UL] * h[0UL] - u[2UL] * h[1UL] + u[0UL] * h[3UL]),
347 0.50 * (u[2UL] * h[0UL] + u[1UL] * h[1UL] - u[0UL] * h[2UL])};
348
349 return hs;
350 }
351}
Generated by doxygen 1.11.0