WPILibC++ 2024.3.2
MatrixLogarithm.h
Go to the documentation of this file.
1// This file is part of Eigen, a lightweight C++ template library
2// for linear algebra.
3//
4// Copyright (C) 2011, 2013 Jitse Niesen <jitse@maths.leeds.ac.uk>
5// Copyright (C) 2011 Chen-Pang He <jdh8@ms63.hinet.net>
6//
7// This Source Code Form is subject to the terms of the Mozilla
8// Public License v. 2.0. If a copy of the MPL was not distributed
9// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
10
11#ifndef EIGEN_MATRIX_LOGARITHM
12#define EIGEN_MATRIX_LOGARITHM
13
14// IWYU pragma: private
16
17namespace Eigen {
18
19namespace internal {
20
21template <typename Scalar>
23 static const int value = 3;
24};
25
26template <typename Scalar>
28 typedef typename NumTraits<Scalar>::Real RealScalar;
29 static const int value = std::numeric_limits<RealScalar>::digits <= 24 ? 5 : // single precision
30 std::numeric_limits<RealScalar>::digits <= 53 ? 7
31 : // double precision
32 std::numeric_limits<RealScalar>::digits <= 64 ? 8
33 : // extended precision
34 std::numeric_limits<RealScalar>::digits <= 106 ? 10
35 : // double-double
37};
38
39/** \brief Compute logarithm of 2x2 triangular matrix. */
40template <typename MatrixType>
41void matrix_log_compute_2x2(const MatrixType& A, MatrixType& result) {
42 typedef typename MatrixType::Scalar Scalar;
43 typedef typename MatrixType::RealScalar RealScalar;
44 using std::abs;
45 using std::ceil;
46 using std::imag;
47 using std::log;
48
49 Scalar logA00 = log(A(0, 0));
50 Scalar logA11 = log(A(1, 1));
51
52 result(0, 0) = logA00;
53 result(1, 0) = Scalar(0);
54 result(1, 1) = logA11;
55
56 Scalar y = A(1, 1) - A(0, 0);
57 if (y == Scalar(0)) {
58 result(0, 1) = A(0, 1) / A(0, 0);
59 } else if ((abs(A(0, 0)) < RealScalar(0.5) * abs(A(1, 1))) || (abs(A(0, 0)) > 2 * abs(A(1, 1)))) {
60 result(0, 1) = A(0, 1) * (logA11 - logA00) / y;
61 } else {
62 // computation in previous branch is inaccurate if A(1,1) \approx A(0,0)
63 RealScalar unwindingNumber = ceil((imag(logA11 - logA00) - RealScalar(EIGEN_PI)) / RealScalar(2 * EIGEN_PI));
64 result(0, 1) = A(0, 1) * (numext::log1p(y / A(0, 0)) + Scalar(0, RealScalar(2 * EIGEN_PI) * unwindingNumber)) / y;
65 }
66}
67
68/* \brief Get suitable degree for Pade approximation. (specialized for RealScalar = float) */
70 const float maxNormForPade[] = {2.5111573934555054e-1 /* degree = 3 */, 4.0535837411880493e-1, 5.3149729967117310e-1};
74 for (; degree <= maxPadeDegree; ++degree)
76 return degree;
77}
78
79/* \brief Get suitable degree for Pade approximation. (specialized for RealScalar = double) */
81 const double maxNormForPade[] = {1.6206284795015624e-2 /* degree = 3 */, 5.3873532631381171e-2, 1.1352802267628681e-1,
82 1.8662860613541288e-1, 2.642960831111435e-1};
86 for (; degree <= maxPadeDegree; ++degree)
88 return degree;
89}
90
91/* \brief Get suitable degree for Pade approximation. (specialized for RealScalar = long double) */
92inline int matrix_log_get_pade_degree(long double normTminusI) {
93#if LDBL_MANT_DIG == 53 // double precision
94 const long double maxNormForPade[] = {1.6206284795015624e-2L /* degree = 3 */, 5.3873532631381171e-2L,
95 1.1352802267628681e-1L, 1.8662860613541288e-1L, 2.642960831111435e-1L};
96#elif LDBL_MANT_DIG <= 64 // extended precision
97 const long double maxNormForPade[] = {5.48256690357782863103e-3L /* degree = 3 */,
98 2.34559162387971167321e-2L,
99 5.84603923897347449857e-2L,
100 1.08486423756725170223e-1L,
101 1.68385767881294446649e-1L,
102 2.32777776523703892094e-1L};
103#elif LDBL_MANT_DIG <= 106 // double-double
104 const long double maxNormForPade[] = {8.58970550342939562202529664318890e-5L /* degree = 3 */,
105 9.34074328446359654039446552677759e-4L,
106 4.26117194647672175773064114582860e-3L,
107 1.21546224740281848743149666560464e-2L,
108 2.61100544998339436713088248557444e-2L,
109 4.66170074627052749243018566390567e-2L,
110 7.32585144444135027565872014932387e-2L,
111 1.05026503471351080481093652651105e-1L};
113 const long double maxNormForPade[] = {4.7419931187193005048501568167858103e-5L /* degree = 3 */,
114 5.8853168473544560470387769480192666e-4L,
115 2.9216120366601315391789493628113520e-3L,
116 8.8415758124319434347116734705174308e-3L,
117 1.9850836029449446668518049562565291e-2L,
118 3.6688019729653446926585242192447447e-2L,
119 5.9290962294020186998954055264528393e-2L,
120 8.6998436081634343903250580992127677e-2L,
121 1.1880960220216759245467951592883642e-1L};
122#endif
126 for (; degree <= maxPadeDegree; ++degree)
128 return degree;
129}
130
131/* \brief Compute Pade approximation to matrix logarithm */
132template <typename MatrixType>
133void matrix_log_compute_pade(MatrixType& result, const MatrixType& T, int degree) {
134 typedef typename NumTraits<typename MatrixType::Scalar>::Real RealScalar;
135 const int minPadeDegree = 3;
136 const int maxPadeDegree = 11;
138 // FIXME this creates float-conversion-warnings if these are enabled.
139 // Either manually convert each value, or disable the warning locally
140 const RealScalar nodes[][maxPadeDegree] = {
141 {0.1127016653792583114820734600217600L, 0.5000000000000000000000000000000000L, // degree 3
142 0.8872983346207416885179265399782400L},
143 {0.0694318442029737123880267555535953L, 0.3300094782075718675986671204483777L, // degree 4
144 0.6699905217924281324013328795516223L, 0.9305681557970262876119732444464048L},
145 {0.0469100770306680036011865608503035L, 0.2307653449471584544818427896498956L, // degree 5
146 0.5000000000000000000000000000000000L, 0.7692346550528415455181572103501044L,
147 0.9530899229693319963988134391496965L},
148 {0.0337652428984239860938492227530027L, 0.1693953067668677431693002024900473L, // degree 6
149 0.3806904069584015456847491391596440L, 0.6193095930415984543152508608403560L,
150 0.8306046932331322568306997975099527L, 0.9662347571015760139061507772469973L},
151 {0.0254460438286207377369051579760744L, 0.1292344072003027800680676133596058L, // degree 7
152 0.2970774243113014165466967939615193L, 0.5000000000000000000000000000000000L,
153 0.7029225756886985834533032060384807L, 0.8707655927996972199319323866403942L,
154 0.9745539561713792622630948420239256L},
155 {0.0198550717512318841582195657152635L, 0.1016667612931866302042230317620848L, // degree 8
156 0.2372337950418355070911304754053768L, 0.4082826787521750975302619288199080L,
157 0.5917173212478249024697380711800920L, 0.7627662049581644929088695245946232L,
158 0.8983332387068133697957769682379152L, 0.9801449282487681158417804342847365L},
159 {0.0159198802461869550822118985481636L, 0.0819844463366821028502851059651326L, // degree 9
160 0.1933142836497048013456489803292629L, 0.3378732882980955354807309926783317L,
161 0.5000000000000000000000000000000000L, 0.6621267117019044645192690073216683L,
162 0.8066857163502951986543510196707371L, 0.9180155536633178971497148940348674L,
163 0.9840801197538130449177881014518364L},
164 {0.0130467357414141399610179939577740L, 0.0674683166555077446339516557882535L, // degree 10
165 0.1602952158504877968828363174425632L, 0.2833023029353764046003670284171079L,
166 0.4255628305091843945575869994351400L, 0.5744371694908156054424130005648600L,
167 0.7166976970646235953996329715828921L, 0.8397047841495122031171636825574368L,
168 0.9325316833444922553660483442117465L, 0.9869532642585858600389820060422260L},
169 {0.0108856709269715035980309994385713L, 0.0564687001159523504624211153480364L, // degree 11
170 0.1349239972129753379532918739844233L, 0.2404519353965940920371371652706952L,
171 0.3652284220238275138342340072995692L, 0.5000000000000000000000000000000000L,
172 0.6347715779761724861657659927004308L, 0.7595480646034059079628628347293048L,
173 0.8650760027870246620467081260155767L, 0.9435312998840476495375788846519636L,
174 0.9891143290730284964019690005614287L}};
175
176 const RealScalar weights[][maxPadeDegree] = {
177 {0.2777777777777777777777777777777778L, 0.4444444444444444444444444444444444L, // degree 3
178 0.2777777777777777777777777777777778L},
179 {0.1739274225687269286865319746109997L, 0.3260725774312730713134680253890003L, // degree 4
180 0.3260725774312730713134680253890003L, 0.1739274225687269286865319746109997L},
181 {0.1184634425280945437571320203599587L, 0.2393143352496832340206457574178191L, // degree 5
182 0.2844444444444444444444444444444444L, 0.2393143352496832340206457574178191L,
183 0.1184634425280945437571320203599587L},
184 {0.0856622461895851725201480710863665L, 0.1803807865240693037849167569188581L, // degree 6
185 0.2339569672863455236949351719947755L, 0.2339569672863455236949351719947755L,
186 0.1803807865240693037849167569188581L, 0.0856622461895851725201480710863665L},
187 {0.0647424830844348466353057163395410L, 0.1398526957446383339507338857118898L, // degree 7
188 0.1909150252525594724751848877444876L, 0.2089795918367346938775510204081633L,
189 0.1909150252525594724751848877444876L, 0.1398526957446383339507338857118898L,
190 0.0647424830844348466353057163395410L},
191 {0.0506142681451881295762656771549811L, 0.1111905172266872352721779972131204L, // degree 8
192 0.1568533229389436436689811009933007L, 0.1813418916891809914825752246385978L,
193 0.1813418916891809914825752246385978L, 0.1568533229389436436689811009933007L,
194 0.1111905172266872352721779972131204L, 0.0506142681451881295762656771549811L},
195 {0.0406371941807872059859460790552618L, 0.0903240803474287020292360156214564L, // degree 9
196 0.1303053482014677311593714347093164L, 0.1561735385200014200343152032922218L,
197 0.1651196775006298815822625346434870L, 0.1561735385200014200343152032922218L,
198 0.1303053482014677311593714347093164L, 0.0903240803474287020292360156214564L,
199 0.0406371941807872059859460790552618L},
200 {0.0333356721543440687967844049466659L, 0.0747256745752902965728881698288487L, // degree 10
201 0.1095431812579910219977674671140816L, 0.1346333596549981775456134607847347L,
202 0.1477621123573764350869464973256692L, 0.1477621123573764350869464973256692L,
203 0.1346333596549981775456134607847347L, 0.1095431812579910219977674671140816L,
204 0.0747256745752902965728881698288487L, 0.0333356721543440687967844049466659L},
205 {0.0278342835580868332413768602212743L, 0.0627901847324523123173471496119701L, // degree 11
206 0.0931451054638671257130488207158280L, 0.1165968822959952399592618524215876L,
207 0.1314022722551233310903444349452546L, 0.1364625433889503153572417641681711L,
208 0.1314022722551233310903444349452546L, 0.1165968822959952399592618524215876L,
209 0.0931451054638671257130488207158280L, 0.0627901847324523123173471496119701L,
210 0.0278342835580868332413768602212743L}};
211
212 MatrixType TminusI = T - MatrixType::Identity(T.rows(), T.rows());
213 result.setZero(T.rows(), T.rows());
214 for (int k = 0; k < degree; ++k) {
215 RealScalar weight = weights[degree - minPadeDegree][k];
216 RealScalar node = nodes[degree - minPadeDegree][k];
217 result +=
218 weight *
219 (MatrixType::Identity(T.rows(), T.rows()) + node * TminusI).template triangularView<Upper>().solve(TminusI);
220 }
221}
222
223/** \brief Compute logarithm of triangular matrices with size > 2.
224 * \details This uses a inverse scale-and-square algorithm. */
225template <typename MatrixType>
226void matrix_log_compute_big(const MatrixType& A, MatrixType& result) {
227 typedef typename MatrixType::Scalar Scalar;
228 typedef typename NumTraits<Scalar>::Real RealScalar;
229 using std::pow;
230
231 int numberOfSquareRoots = 0;
232 int numberOfExtraSquareRoots = 0;
233 int degree;
234 MatrixType T = A, sqrtT;
235
237 const RealScalar maxNormForPade = RealScalar(maxPadeDegree <= 5 ? 5.3149729967117310e-1L : // single precision
238 maxPadeDegree <= 7 ? 2.6429608311114350e-1L
239 : // double precision
240 maxPadeDegree <= 8 ? 2.32777776523703892094e-1L
241 : // extended precision
242 maxPadeDegree <= 10 ? 1.05026503471351080481093652651105e-1L
243 : // double-double
245
246 while (true) {
247 RealScalar normTminusI = (T - MatrixType::Identity(T.rows(), T.rows())).cwiseAbs().colwise().sum().maxCoeff();
248 if (normTminusI < maxNormForPade) {
250 int degree2 = matrix_log_get_pade_degree(normTminusI / RealScalar(2));
251 if ((degree - degree2 <= 1) || (numberOfExtraSquareRoots == 1)) break;
252 ++numberOfExtraSquareRoots;
253 }
254 matrix_sqrt_triangular(T, sqrtT);
255 T = sqrtT.template triangularView<Upper>();
256 ++numberOfSquareRoots;
257 }
258
260 result *= pow(RealScalar(2), RealScalar(numberOfSquareRoots)); // TODO replace by bitshift if possible
261}
262
263/** \ingroup MatrixFunctions_Module
264 * \class MatrixLogarithmAtomic
265 * \brief Helper class for computing matrix logarithm of atomic matrices.
266 *
267 * Here, an atomic matrix is a triangular matrix whose diagonal entries are close to each other.
268 *
269 * \sa class MatrixFunctionAtomic, MatrixBase::log()
270 */
271template <typename MatrixType>
273 public:
274 /** \brief Compute matrix logarithm of atomic matrix
275 * \param[in] A argument of matrix logarithm, should be upper triangular and atomic
276 * \returns The logarithm of \p A.
277 */
278 MatrixType compute(const MatrixType& A);
279};
280
281template <typename MatrixType>
282MatrixType MatrixLogarithmAtomic<MatrixType>::compute(const MatrixType& A) {
283 using std::log;
284 MatrixType result(A.rows(), A.rows());
285 if (A.rows() == 1)
286 result(0, 0) = log(A(0, 0));
287 else if (A.rows() == 2)
288 matrix_log_compute_2x2(A, result);
289 else
290 matrix_log_compute_big(A, result);
291 return result;
292}
293
294} // end of namespace internal
295
296/** \ingroup MatrixFunctions_Module
297 *
298 * \brief Proxy for the matrix logarithm of some matrix (expression).
299 *
300 * \tparam Derived Type of the argument to the matrix function.
301 *
302 * This class holds the argument to the matrix function until it is
303 * assigned or evaluated for some other reason (so the argument
304 * should not be changed in the meantime). It is the return type of
305 * MatrixBase::log() and most of the time this is the only way it
306 * is used.
307 */
308template <typename Derived>
309class MatrixLogarithmReturnValue : public ReturnByValue<MatrixLogarithmReturnValue<Derived> > {
310 public:
311 typedef typename Derived::Scalar Scalar;
312 typedef typename Derived::Index Index;
313
314 protected:
316
317 public:
318 /** \brief Constructor.
319 *
320 * \param[in] A %Matrix (expression) forming the argument of the matrix logarithm.
321 */
322 explicit MatrixLogarithmReturnValue(const Derived& A) : m_A(A) {}
323
324 /** \brief Compute the matrix logarithm.
325 *
326 * \param[out] result Logarithm of \c A, where \c A is as specified in the constructor.
327 */
328 template <typename ResultType>
329 inline void evalTo(ResultType& result) const {
330 typedef typename internal::nested_eval<Derived, 10>::type DerivedEvalType;
331 typedef internal::remove_all_t<DerivedEvalType> DerivedEvalTypeClean;
332 typedef internal::traits<DerivedEvalTypeClean> Traits;
333 typedef std::complex<typename NumTraits<Scalar>::Real> ComplexScalar;
334 typedef Matrix<ComplexScalar, Dynamic, Dynamic, 0, Traits::RowsAtCompileTime, Traits::ColsAtCompileTime>
335 DynMatrixType;
337 AtomicType atomic;
338
340 }
341
342 Index rows() const { return m_A.rows(); }
343 Index cols() const { return m_A.cols(); }
344
345 private:
346 const DerivedNested m_A;
347};
348
349namespace internal {
350template <typename Derived>
351struct traits<MatrixLogarithmReturnValue<Derived> > {
352 typedef typename Derived::PlainObject ReturnType;
353};
354} // namespace internal
355
356/********** MatrixBase method **********/
357
358template <typename Derived>
360 eigen_assert(rows() == cols());
361 return MatrixLogarithmReturnValue<Derived>(derived());
362}
363
364} // end namespace Eigen
365
366#endif // EIGEN_MATRIX_LOGARITHM
Proxy for the matrix logarithm of some matrix (expression).
Definition: MatrixLogarithm.h:309
Index rows() const
Definition: MatrixLogarithm.h:342
internal::ref_selector< Derived >::type DerivedNested
Definition: MatrixLogarithm.h:315
Index cols() const
Definition: MatrixLogarithm.h:343
Derived::Index Index
Definition: MatrixLogarithm.h:312
void evalTo(ResultType &result) const
Compute the matrix logarithm.
Definition: MatrixLogarithm.h:329
Derived::Scalar Scalar
Definition: MatrixLogarithm.h:311
MatrixLogarithmReturnValue(const Derived &A)
Constructor.
Definition: MatrixLogarithm.h:322
Helper class for computing matrix logarithm of atomic matrices.
Definition: MatrixLogarithm.h:272
MatrixType compute(const MatrixType &A)
Compute matrix logarithm of atomic matrix.
Definition: MatrixLogarithm.h:282
void matrix_sqrt_triangular(const MatrixType &arg, ResultType &result)
Compute matrix square root of triangular matrix.
Definition: MatrixSquareRoot.h:194
UnitType abs(const UnitType x) noexcept
Compute absolute value.
Definition: math.h:721
dimensionless::scalar_t log(const ScalarUnit x) noexcept
Compute natural logarithm.
Definition: math.h:349
dimensionless::scalar_t log1p(const ScalarUnit x) noexcept
Compute logarithm plus one.
Definition: math.h:437
UnitType ceil(const UnitType x) noexcept
Round up value.
Definition: math.h:528
void matrix_log_compute_2x2(const MatrixType &A, MatrixType &result)
Compute logarithm of 2x2 triangular matrix.
Definition: MatrixLogarithm.h:41
void matrix_log_compute_pade(MatrixType &result, const MatrixType &T, int degree)
Definition: MatrixLogarithm.h:133
void matrix_log_compute_big(const MatrixType &A, MatrixType &result)
Compute logarithm of triangular matrices with size > 2.
Definition: MatrixLogarithm.h:226
Definition: MatrixLogarithm.h:69
Definition: MatrixSquareRoot.h:16
type
Definition: core.h:556
auto pow(const UnitType &value) noexcept -> unit_t< typename units::detail::power_of_unit< power, typename units::traits::unit_t_traits< UnitType >::unit_type >::type, typename units::traits::unit_t_traits< UnitType >::underlying_type, linear_scale >
computes the value of value raised to the power
Definition: base.h:2806
degree
Definition: angle.h:43
static void run(const MatrixType &A, AtomicType &atomic, ResultType &result)
Compute the matrix function.
Definition: MatrixLogarithm.h:27
static const int value
Definition: MatrixLogarithm.h:29
NumTraits< Scalar >::Real RealScalar
Definition: MatrixLogarithm.h:28
Definition: MatrixLogarithm.h:22
static const int value
Definition: MatrixLogarithm.h:23
Derived::PlainObject ReturnType
Definition: MatrixLogarithm.h:352