|
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
|
#ifndef DataFormat_Math_invertPosDefMatrix_H
#define DataFormat_Math_invertPosDefMatrix_H
#define SMATRIX_USE_CONSTEXPR
#include "Math/SMatrix.h"
#include "Math/CholeskyDecomp.h"
#include <type_traits>
template <typename T, unsigned int N>
inline bool invertPosDefMatrix(ROOT::Math::SMatrix<T, N, N, ROOT::Math::MatRepSym<T, N> >& m) {
ROOT::Math::CholeskyDecomp<T, N> decomp(m);
if (!decomp) {
return m.Invert();
} else
decomp.Invert(m);
return true;
}
template <typename PDM2>
void fastInvertPDM2(PDM2& mm) {
auto m = mm.Array();
constexpr typename std::remove_reference<decltype(m[0])>::type one = 1.;
auto c0 = one / m[0];
auto c1 = m[1] * m[1] * c0;
auto c2 = one / (m[2] - c1);
auto li21 = c1 * c0 * c2;
m[0] = li21 + c0;
m[1] = -m[1] * c0 * c2;
m[2] = c2;
}
template <>
inline bool invertPosDefMatrix<double, 1>(ROOT::Math::SMatrix<double, 1, 1, ROOT::Math::MatRepSym<double, 1> >& m) {
m(0, 0) = 1. / m(0, 0);
return true;
}
template <>
inline bool invertPosDefMatrix<float, 1>(ROOT::Math::SMatrix<float, 1, 1, ROOT::Math::MatRepSym<float, 1> >& m) {
m(0, 0) = 1.f / m(0, 0);
return true;
}
template <>
inline bool invertPosDefMatrix<double, 2>(ROOT::Math::SMatrix<double, 2, 2, ROOT::Math::MatRepSym<double, 2> >& m) {
fastInvertPDM2(m);
return true;
}
template <>
inline bool invertPosDefMatrix<float, 2>(ROOT::Math::SMatrix<float, 2, 2, ROOT::Math::MatRepSym<float, 2> >& m) {
fastInvertPDM2(m);
return true;
}
template <typename T, unsigned int N>
inline bool invertPosDefMatrix(ROOT::Math::SMatrix<T, N, N, ROOT::Math::MatRepSym<T, N> > const& mIn,
ROOT::Math::SMatrix<T, N, N, ROOT::Math::MatRepSym<T, N> >& mOut) {
ROOT::Math::CholeskyDecomp<T, N> decomp(mIn);
if (!decomp) {
mOut = mIn;
return mOut.Invert();
} else
decomp.Invert(mOut);
return true;
}
#endif
|