File indexing completed on 2025-09-18 08:11:58
0001
0002
0003
0004
0005
0006
0007
0008
0009 #pragma once
0010
0011 #include "Acts/Definitions/Algebra.hpp"
0012 #include "Acts/Utilities/ArrayHelpers.hpp"
0013 #include "Acts/Utilities/MathHelpers.hpp"
0014
0015 #include <bitset>
0016 #include <optional>
0017
0018 #include "Eigen/Dense"
0019
0020 namespace Acts {
0021
0022
0023
0024
0025
0026
0027
0028
0029 template <typename MatrixType>
0030 MatrixType bitsetToMatrix(const std::bitset<MatrixType::RowsAtCompileTime *
0031 MatrixType::ColsAtCompileTime>
0032 bs) {
0033 constexpr int rows = MatrixType::RowsAtCompileTime;
0034 constexpr int cols = MatrixType::ColsAtCompileTime;
0035
0036 static_assert(rows != -1 && cols != -1,
0037 "bitsetToMatrix does not support dynamic matrices");
0038
0039 MatrixType m;
0040 auto* p = m.data();
0041 for (std::size_t i = 0; i < rows * cols; i++) {
0042 p[i] = bs[rows * cols - 1 - i];
0043 }
0044 return m;
0045 }
0046
0047
0048
0049
0050
0051
0052
0053 template <typename Derived>
0054 auto matrixToBitset(const Eigen::PlainObjectBase<Derived>& m) {
0055 using MatrixType = Eigen::PlainObjectBase<Derived>;
0056 constexpr std::size_t rows = MatrixType::RowsAtCompileTime;
0057 constexpr std::size_t cols = MatrixType::ColsAtCompileTime;
0058
0059 std::bitset<rows * cols> res;
0060
0061 auto* p = m.data();
0062 for (std::size_t i = 0; i < rows * cols; i++) {
0063 res[rows * cols - 1 - i] = static_cast<bool>(p[i]);
0064 }
0065
0066 return res;
0067 }
0068
0069
0070
0071
0072
0073
0074
0075
0076
0077
0078
0079 template <typename A, typename B>
0080 inline ActsMatrix<A::RowsAtCompileTime, B::ColsAtCompileTime> blockedMult(
0081 const A& a, const B& b) {
0082
0083
0084 constexpr int M = A::RowsAtCompileTime;
0085 constexpr int N = A::ColsAtCompileTime;
0086 constexpr int P = B::ColsAtCompileTime;
0087
0088
0089
0090 static_assert(N == B::RowsAtCompileTime);
0091
0092 if constexpr (M <= 4 && N <= 4 && P <= 4) {
0093
0094
0095 return a * b;
0096 } else {
0097
0098
0099
0100
0101
0102
0103
0104
0105
0106
0107
0108
0109
0110
0111
0112
0113
0114
0115
0116
0117
0118
0119
0120
0121
0122
0123
0124
0125
0126
0127
0128
0129 constexpr int M1 = M / 2;
0130 constexpr int M2 = (M + 1) / 2;
0131 constexpr int N1 = N / 2;
0132 constexpr int N2 = (N + 1) / 2;
0133 constexpr int P1 = P / 2;
0134 constexpr int P2 = (P + 1) / 2;
0135
0136
0137
0138 ActsMatrix<M, P> r;
0139
0140
0141 r.template topLeftCorner<M1, P1>().noalias() =
0142 a.template topLeftCorner<M1, N1>() *
0143 b.template topLeftCorner<N1, P1>() +
0144 a.template topRightCorner<M1, N2>() *
0145 b.template bottomLeftCorner<N2, P1>();
0146
0147
0148 r.template topRightCorner<M1, P2>().noalias() =
0149 a.template topLeftCorner<M1, N1>() *
0150 b.template topRightCorner<N1, P2>() +
0151 a.template topRightCorner<M1, N2>() *
0152 b.template bottomRightCorner<N2, P2>();
0153
0154
0155 r.template bottomLeftCorner<M2, P1>().noalias() =
0156 a.template bottomLeftCorner<M2, N1>() *
0157 b.template topLeftCorner<N1, P1>() +
0158 a.template bottomRightCorner<M2, N2>() *
0159 b.template bottomLeftCorner<N2, P1>();
0160
0161
0162 r.template bottomRightCorner<M2, P2>().noalias() =
0163 a.template bottomLeftCorner<M2, N1>() *
0164 b.template topRightCorner<N1, P2>() +
0165 a.template bottomRightCorner<M2, N2>() *
0166 b.template bottomRightCorner<N2, P2>();
0167
0168 return r;
0169 }
0170 }
0171
0172
0173
0174
0175
0176
0177
0178
0179
0180
0181
0182
0183
0184
0185
0186
0187
0188
0189
0190
0191 template <typename MatrixType, typename ResultType = MatrixType>
0192 std::optional<ResultType> safeInverse(const MatrixType& m) noexcept {
0193 constexpr int rows = MatrixType::RowsAtCompileTime;
0194 constexpr int cols = MatrixType::ColsAtCompileTime;
0195
0196 static_assert(rows == cols);
0197
0198 ResultType result;
0199 bool invertible = false;
0200
0201 if constexpr (rows > 4 || rows == -1) {
0202 Eigen::FullPivLU<MatrixType> mFullPivLU(m);
0203 if (mFullPivLU.isInvertible()) {
0204 invertible = true;
0205 result = mFullPivLU.inverse();
0206 }
0207 } else {
0208 m.computeInverseWithCheck(result, invertible);
0209 }
0210
0211 if (invertible) {
0212 return result;
0213 }
0214
0215 return std::nullopt;
0216 }
0217
0218
0219
0220
0221 template <typename T>
0222 struct ExpSafeLimit {};
0223 template <>
0224 struct ExpSafeLimit<double> {
0225 constexpr static double value = 500.0;
0226 };
0227 template <>
0228 struct ExpSafeLimit<float> {
0229 constexpr static float value = 50.0;
0230 };
0231
0232
0233
0234
0235
0236
0237
0238 template <typename T>
0239 constexpr T safeExp(T val) noexcept {
0240 constexpr T maxExponent = ExpSafeLimit<T>::value;
0241 constexpr T minExponent = -maxExponent;
0242 if (val < minExponent) {
0243 return 0.0;
0244 }
0245
0246 if (val > maxExponent) {
0247 return std::numeric_limits<T>::infinity();
0248 }
0249
0250 return std::exp(val);
0251 }
0252
0253
0254
0255
0256
0257 template <std::size_t N>
0258 constexpr std::size_t vecIdxFromSymMat(const std::size_t i, const std::size_t k)
0259 requires(N > 0)
0260 {
0261 assert(i < N);
0262 assert(k < N);
0263 if (k > i) {
0264 return vecIdxFromSymMat<N>(k, i);
0265 }
0266 return sumUpToN(i) + k;
0267 }
0268
0269
0270
0271
0272
0273
0274 template <std::size_t N>
0275 constexpr std::array<std::size_t, 2> symMatIndices(const std::size_t k)
0276 requires(N > 1)
0277 {
0278 assert(k < sumUpToN(N));
0279 constexpr std::size_t bound = sumUpToN(N - 1);
0280 if (k >= bound) {
0281 return std::array<std::size_t, 2>{N - 1, k - bound};
0282 }
0283 if constexpr (N > 2) {
0284 return symMatIndices<N - 1>(k);
0285 }
0286 return filledArray<std::size_t, 2>(0);
0287 }
0288
0289 }