24#include <fmt/format.h>
40template<std::
floating_po
int T>
41constexpr T
round(
const T& value,
size_t digits)
43 auto factor = std::pow(10, digits);
44 return std::round(value * factor) / factor;
51template<std::
floating_po
int T>
54 if (value == 0.0) {
return 0.0; }
56 auto absVal = gcem::abs(value);
57 auto log10 =
static_cast<int32_t
>(gcem::log10(absVal));
58 auto exp = log10 + (log10 > 0 || (log10 == 0 && absVal >= 1.0));
59 auto fac =
static_cast<T
>(digits) -
static_cast<T
>(exp);
61 auto factor =
static_cast<T
>(gcem::pow(10.0, fac));
64 return static_cast<T
>(gcem::round(value * factor) / factor);
73template<std::
integral Out,
size_t Bits, std::
integral In>
76 static_assert(Bits <
sizeof(In) * 8);
77 static_assert(Bits <
sizeof(Out) * 8);
79 constexpr size_t N =
sizeof(Out) * 8 - Bits;
80 return static_cast<Out
>(
static_cast<Out
>((in &
static_cast<In
>(gcem::pow(2, Bits) - 1)) << N) >> N);
89template<
typename Derived>
95 Eigen::Matrix<typename Derived::Scalar, 3, 3> skewMat;
96 skewMat << 0, -a(2), a(1),
107template<
typename Derived>
113 Eigen::Matrix<typename Derived::Scalar, 3, 3> skewMat2;
114 skewMat2 << std::pow(a(2), 2) + std::pow(a(1), 2), a(0) * a(1), a(0) * a(2),
115 a(0) * a(1), std::pow(a(2), 2) + std::pow(a(0), 2), a(1) * a(2),
116 a(0) * a(2), a(1) * a(2), std::pow(a(0), 2) + std::pow(a(1), 2);
122template<std::
floating_po
int T>
125 return 1.0 / std::cos(x);
129template<std::
floating_po
int T>
132 return 1.0 / std::sin(x);
141 return (T(0) < val) - (val < T(0));
148template<
typename Derived>
149typename Derived::PlainObject
expm(
const Eigen::MatrixBase<Derived>& X,
size_t order)
151 INS_ASSERT_USER_ERROR(X.rows() == X.cols(),
"Matrix exponential calculation only possible for n x n matrices");
153 typename Derived::PlainObject e_X;
155 if constexpr (Derived::RowsAtCompileTime == Eigen::Dynamic)
157 e_X = Eigen::MatrixBase<Derived>::Identity(X.rows(), X.cols());
161 e_X = Eigen::MatrixBase<Derived>::Identity();
163 typename Derived::PlainObject Xpower = X;
164 for (
size_t i = 1; i <= order; i++)
183template<
typename Derived>
184std::optional<std::pair<Eigen::Matrix<typename Derived::Scalar, Derived::RowsAtCompileTime, Derived::ColsAtCompileTime>,
185 Eigen::Vector<typename Derived::Scalar, Derived::RowsAtCompileTime>>>
190 auto n = Qmatrix.rows();
191 Eigen::Matrix<typename Derived::Scalar, Derived::RowsAtCompileTime, Derived::ColsAtCompileTime> Q = Qmatrix.template triangularView<Eigen::Lower>();
192 Eigen::Matrix<typename Derived::Scalar, Derived::RowsAtCompileTime, Derived::ColsAtCompileTime> L;
193 Eigen::Vector<typename Derived::Scalar, Derived::RowsAtCompileTime> D;
195 if constexpr (Derived::RowsAtCompileTime == Eigen::Dynamic)
197 L = Eigen::Matrix<typename Derived::Scalar, Eigen::Dynamic, Eigen::Dynamic>::Zero(n, n);
202 L = Eigen::Matrix<typename Derived::Scalar, Derived::RowsAtCompileTime, Derived::ColsAtCompileTime>::Zero();
206 for (Eigen::Index i = n - 1; i >= 0; i--)
209 if (Q(i, i) <= 0.0) {
return {}; }
210 L(i, seq(0, i)) = Q(i, seq(0, i)) / std::sqrt(Q(i, i));
212 for (Eigen::Index j = 0; j <= i - 1; j++)
214 Q(j, seq(0, j)) -= L(i, seq(0, j)) * L(i, j);
216 L(i, seq(0, i)) /= L(i, i);
219 return std::make_pair(L, D);
227template<
typename Derived>
228std::optional<std::pair<Eigen::Matrix<typename Derived::Scalar, Derived::RowsAtCompileTime, Derived::ColsAtCompileTime>,
229 Eigen::Vector<typename Derived::Scalar, Derived::RowsAtCompileTime>>>
235 typename Derived::PlainObject L = Q.template triangularView<Eigen::Lower>();
236 Eigen::Vector<typename Derived::Scalar, Derived::RowsAtCompileTime> D;
239 if constexpr (Derived::RowsAtCompileTime == Eigen::Dynamic) { D.setZero(n); }
240 else { D.setZero(); }
242 for (Eigen::Index j = n - 1; j >= 0; j--)
244 for (Eigen::Index i = n - 1; i >= j + 1; i--)
246 L(i, j) = (L(i, j) - L(seq(i + 1, n - 1), j).dot(L(seq(i + 1, n - 1), i))) / L(i, i);
248 double t = L(j, j) - L(seq(j + 1, n - 1), j).dot(L(seq(j + 1, n - 1), j));
249 if (t <= 0.0) {
return {}; }
250 double c = t / L(j, j);
251 cmin = std::min(c, cmin);
252 L(j, j) = std::sqrt(t);
254 for (Eigen::Index i = 0; i < n; i++)
256 L.row(i).leftCols(i) /= L(i, i);
257 D(i) = std::pow(L(i, i), 2.0);
261 return std::make_pair(L, D);
272template<
typename DerivedA,
typename DerivedQ>
275 static_assert(DerivedA::ColsAtCompileTime == Eigen::Dynamic || DerivedA::ColsAtCompileTime == 1);
280 return a.transpose() * Q.inverse() * a;
313template<
typename Derived>
314[[nodiscard]]
typename Derived::PlainObject
inverseSqrt(
const Eigen::MatrixBase<Derived>& matrix)
317 if constexpr (std::is_floating_point_v<typename Derived::Scalar>)
319 return matrix.inverse().sqrt();
323 Eigen::JacobiSVD<Eigen::MatrixX<typename Derived::Scalar>> svd(matrix.inverse(), Eigen::ComputeFullV);
324 typename Derived::PlainObject sqrtInverse = svd.matrixV() * svd.singularValues().cwiseSqrt().asDiagonal() * svd.matrixV().transpose();
342 return -1.0 * fabs(x);
350template<
typename Derived>
351typename Derived::PlainObject
lerp(
const Eigen::MatrixBase<Derived>& a,
const Eigen::MatrixBase<Derived>& b,
auto t)
353 return a + t * (b - a);
369 auto i =
static_cast<size_t>(std::distance(data.begin(), std::upper_bound(data.begin(), data.end(), value)));
371 if (i == data.size() - 1) { i--; }
372 const auto& lb = data.at(i);
373 const auto& ub = data.at(i + 1);
374 double t = (value - lb) / (ub - lb);
376 return { .l = i, .u = i + 1, .t = t };
395 const auto& c00,
const auto& c10,
396 const auto& c01,
const auto& c11)
398 auto a = c00 * (1.0 - tx) + c10 * tx;
399 auto b = c01 * (1.0 - tx) + c11 * tx;
400 return a * (1.0 - ty) + b * ty;
#define INS_ASSERT_USER_ERROR(_EXP, _MSG)
Assert function with message.
T sec(const T &x)
Calculates the secant of a value (sec(x) = csc(pi/2 - x) = 1 / cos(x))
LerpSearchResult lerpSearch(const auto &data, const auto &value)
Searches the value in the data container.
T csc(const T &x)
Calculates the cosecant of a value (csc(x) = sec(pi/2 - x) = 1 / sin(x))
DerivedA::Scalar squaredNormVectorMatrix(const Eigen::MatrixBase< DerivedA > &a, const Eigen::MatrixBase< DerivedQ > &Q)
Calculates the squared norm of the vector and matrix.
Derived::PlainObject expm(const Eigen::MatrixBase< Derived > &X, size_t order)
Calculates the state transition matrix 𝚽 limited to specified order in 𝐅𝜏ₛ
int sgn(const T &val)
Returns the sign of the given value.
std::optional< std::pair< Eigen::Matrix< typename Derived::Scalar, Derived::RowsAtCompileTime, Derived::ColsAtCompileTime >, Eigen::Vector< typename Derived::Scalar, Derived::RowsAtCompileTime > > > LtDLdecomp_outerProduct(const Eigen::MatrixBase< Derived > &Qmatrix)
Find (L^T D L)-decomposition of Q-matrix via outer product method.
T sign(const T &x, const T &y)
Change the sign of x according to the value of y.
Eigen::Matrix< typename Derived::Scalar, 3, 3 > skewSymmetricMatrixSquared(const Eigen::MatrixBase< Derived > &a)
Calculates the square of a skew symmetric matrix of the given vector.
auto bilinearInterpolation(const auto &tx, const auto &ty, const auto &c00, const auto &c10, const auto &c01, const auto &c11)
Bilinear interpolation.
constexpr T roundSignificantDigits(T value, size_t digits)
Round the number to the specified amount of significant digits.
Derived::PlainObject lerp(const Eigen::MatrixBase< Derived > &a, const Eigen::MatrixBase< Derived > &b, auto t)
Linear interpolation between vectors.
double calcEllipticalIntegral(double phi, double m)
Calculates the incomplete elliptical integral of the second kind.
Eigen::Matrix< typename Derived::Scalar, 3, 3 > skewSymmetricMatrix(const Eigen::MatrixBase< Derived > &a)
Calculates the skew symmetric matrix of the given vector. This is needed to perform the cross product...
uint64_t factorial(uint64_t n)
Calculates the factorial of an unsigned integer.
double normalCDF(double value)
Calculates the cumulative distribution function (CDF) of the standard normal distribution.
constexpr T round(const T &value, size_t digits)
Round the number to the specified amount of digits.
constexpr Out interpretAs(In in)
Interprets the input integer with certain amount of Bits as Output type. Takes care of sign extension...
Derived::PlainObject inverseSqrt(const Eigen::MatrixBase< Derived > &matrix)
Returns the inverse square root of a matrix.
std::optional< std::pair< Eigen::Matrix< typename Derived::Scalar, Derived::RowsAtCompileTime, Derived::ColsAtCompileTime >, Eigen::Vector< typename Derived::Scalar, Derived::RowsAtCompileTime > > > LtDLdecomp_choleskyFact(const Eigen::MatrixBase< Derived > &Q)
Find (L^T D L)-decomposition of Q-matrix via a backward Cholesky factorization in a bordering method ...
size_t u
Upper bound index (l + 1)
double t
[0, 1] for Interpolation, otherwise Extrapolation
size_t l
Lower bound index.