WPILibC++ 2024.3.2
MatrixSquareRoot.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//
6// This Source Code Form is subject to the terms of the Mozilla
7// Public License v. 2.0. If a copy of the MPL was not distributed
8// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
9
10#ifndef EIGEN_MATRIX_SQUARE_ROOT
11#define EIGEN_MATRIX_SQUARE_ROOT
12
13// IWYU pragma: private
15
16namespace Eigen {
17
18namespace internal {
19
20// pre: T.block(i,i,2,2) has complex conjugate eigenvalues
21// post: sqrtT.block(i,i,2,2) is square root of T.block(i,i,2,2)
22template <typename MatrixType, typename ResultType>
23void matrix_sqrt_quasi_triangular_2x2_diagonal_block(const MatrixType& T, Index i, ResultType& sqrtT) {
24 // TODO: This case (2-by-2 blocks with complex conjugate eigenvalues) is probably hidden somewhere
25 // in EigenSolver. If we expose it, we could call it directly from here.
26 typedef typename traits<MatrixType>::Scalar Scalar;
27 Matrix<Scalar, 2, 2> block = T.template block<2, 2>(i, i);
28 EigenSolver<Matrix<Scalar, 2, 2> > es(block);
29 sqrtT.template block<2, 2>(i, i) =
30 (es.eigenvectors() * es.eigenvalues().cwiseSqrt().asDiagonal() * es.eigenvectors().inverse()).real();
31}
32
33// pre: block structure of T is such that (i,j) is a 1x1 block,
34// all blocks of sqrtT to left of and below (i,j) are correct
35// post: sqrtT(i,j) has the correct value
36template <typename MatrixType, typename ResultType>
37void matrix_sqrt_quasi_triangular_1x1_off_diagonal_block(const MatrixType& T, Index i, Index j, ResultType& sqrtT) {
38 typedef typename traits<MatrixType>::Scalar Scalar;
39 Scalar tmp = (sqrtT.row(i).segment(i + 1, j - i - 1) * sqrtT.col(j).segment(i + 1, j - i - 1)).value();
40 sqrtT.coeffRef(i, j) = (T.coeff(i, j) - tmp) / (sqrtT.coeff(i, i) + sqrtT.coeff(j, j));
41}
42
43// similar to compute1x1offDiagonalBlock()
44template <typename MatrixType, typename ResultType>
45void matrix_sqrt_quasi_triangular_1x2_off_diagonal_block(const MatrixType& T, Index i, Index j, ResultType& sqrtT) {
46 typedef typename traits<MatrixType>::Scalar Scalar;
47 Matrix<Scalar, 1, 2> rhs = T.template block<1, 2>(i, j);
48 if (j - i > 1) rhs -= sqrtT.block(i, i + 1, 1, j - i - 1) * sqrtT.block(i + 1, j, j - i - 1, 2);
49 Matrix<Scalar, 2, 2> A = sqrtT.coeff(i, i) * Matrix<Scalar, 2, 2>::Identity();
50 A += sqrtT.template block<2, 2>(j, j).transpose();
51 sqrtT.template block<1, 2>(i, j).transpose() = A.fullPivLu().solve(rhs.transpose());
52}
53
54// similar to compute1x1offDiagonalBlock()
55template <typename MatrixType, typename ResultType>
56void matrix_sqrt_quasi_triangular_2x1_off_diagonal_block(const MatrixType& T, Index i, Index j, ResultType& sqrtT) {
57 typedef typename traits<MatrixType>::Scalar Scalar;
58 Matrix<Scalar, 2, 1> rhs = T.template block<2, 1>(i, j);
59 if (j - i > 2) rhs -= sqrtT.block(i, i + 2, 2, j - i - 2) * sqrtT.block(i + 2, j, j - i - 2, 1);
60 Matrix<Scalar, 2, 2> A = sqrtT.coeff(j, j) * Matrix<Scalar, 2, 2>::Identity();
61 A += sqrtT.template block<2, 2>(i, i);
62 sqrtT.template block<2, 1>(i, j) = A.fullPivLu().solve(rhs);
63}
64
65// solves the equation A X + X B = C where all matrices are 2-by-2
66template <typename MatrixType>
67void matrix_sqrt_quasi_triangular_solve_auxiliary_equation(MatrixType& X, const MatrixType& A, const MatrixType& B,
68 const MatrixType& C) {
69 typedef typename traits<MatrixType>::Scalar Scalar;
70 Matrix<Scalar, 4, 4> coeffMatrix = Matrix<Scalar, 4, 4>::Zero();
71 coeffMatrix.coeffRef(0, 0) = A.coeff(0, 0) + B.coeff(0, 0);
72 coeffMatrix.coeffRef(1, 1) = A.coeff(0, 0) + B.coeff(1, 1);
73 coeffMatrix.coeffRef(2, 2) = A.coeff(1, 1) + B.coeff(0, 0);
74 coeffMatrix.coeffRef(3, 3) = A.coeff(1, 1) + B.coeff(1, 1);
75 coeffMatrix.coeffRef(0, 1) = B.coeff(1, 0);
76 coeffMatrix.coeffRef(0, 2) = A.coeff(0, 1);
77 coeffMatrix.coeffRef(1, 0) = B.coeff(0, 1);
78 coeffMatrix.coeffRef(1, 3) = A.coeff(0, 1);
79 coeffMatrix.coeffRef(2, 0) = A.coeff(1, 0);
80 coeffMatrix.coeffRef(2, 3) = B.coeff(1, 0);
81 coeffMatrix.coeffRef(3, 1) = A.coeff(1, 0);
82 coeffMatrix.coeffRef(3, 2) = B.coeff(0, 1);
83
84 Matrix<Scalar, 4, 1> rhs;
85 rhs.coeffRef(0) = C.coeff(0, 0);
86 rhs.coeffRef(1) = C.coeff(0, 1);
87 rhs.coeffRef(2) = C.coeff(1, 0);
88 rhs.coeffRef(3) = C.coeff(1, 1);
89
90 Matrix<Scalar, 4, 1> result;
91 result = coeffMatrix.fullPivLu().solve(rhs);
92
93 X.coeffRef(0, 0) = result.coeff(0);
94 X.coeffRef(0, 1) = result.coeff(1);
95 X.coeffRef(1, 0) = result.coeff(2);
96 X.coeffRef(1, 1) = result.coeff(3);
97}
98
99// similar to compute1x1offDiagonalBlock()
100template <typename MatrixType, typename ResultType>
101void matrix_sqrt_quasi_triangular_2x2_off_diagonal_block(const MatrixType& T, Index i, Index j, ResultType& sqrtT) {
102 typedef typename traits<MatrixType>::Scalar Scalar;
103 Matrix<Scalar, 2, 2> A = sqrtT.template block<2, 2>(i, i);
104 Matrix<Scalar, 2, 2> B = sqrtT.template block<2, 2>(j, j);
105 Matrix<Scalar, 2, 2> C = T.template block<2, 2>(i, j);
106 if (j - i > 2) C -= sqrtT.block(i, i + 2, 2, j - i - 2) * sqrtT.block(i + 2, j, j - i - 2, 2);
107 Matrix<Scalar, 2, 2> X;
109 sqrtT.template block<2, 2>(i, j) = X;
110}
111
112// pre: T is quasi-upper-triangular and sqrtT is a zero matrix of the same size
113// post: the diagonal blocks of sqrtT are the square roots of the diagonal blocks of T
114template <typename MatrixType, typename ResultType>
115void matrix_sqrt_quasi_triangular_diagonal(const MatrixType& T, ResultType& sqrtT) {
116 using std::sqrt;
117 const Index size = T.rows();
118 for (Index i = 0; i < size; i++) {
119 if (i == size - 1 || T.coeff(i + 1, i) == 0) {
120 eigen_assert(T(i, i) >= 0);
121 sqrtT.coeffRef(i, i) = sqrt(T.coeff(i, i));
122 } else {
124 ++i;
125 }
126 }
127}
128
129// pre: T is quasi-upper-triangular and diagonal blocks of sqrtT are square root of diagonal blocks of T.
130// post: sqrtT is the square root of T.
131template <typename MatrixType, typename ResultType>
132void matrix_sqrt_quasi_triangular_off_diagonal(const MatrixType& T, ResultType& sqrtT) {
133 const Index size = T.rows();
134 for (Index j = 1; j < size; j++) {
135 if (T.coeff(j, j - 1) != 0) // if T(j-1:j, j-1:j) is a 2-by-2 block
136 continue;
137 for (Index i = j - 1; i >= 0; i--) {
138 if (i > 0 && T.coeff(i, i - 1) != 0) // if T(i-1:i, i-1:i) is a 2-by-2 block
139 continue;
140 bool iBlockIs2x2 = (i < size - 1) && (T.coeff(i + 1, i) != 0);
141 bool jBlockIs2x2 = (j < size - 1) && (T.coeff(j + 1, j) != 0);
142 if (iBlockIs2x2 && jBlockIs2x2)
144 else if (iBlockIs2x2 && !jBlockIs2x2)
146 else if (!iBlockIs2x2 && jBlockIs2x2)
148 else if (!iBlockIs2x2 && !jBlockIs2x2)
150 }
151 }
152}
153
154} // end of namespace internal
155
156/** \ingroup MatrixFunctions_Module
157 * \brief Compute matrix square root of quasi-triangular matrix.
158 *
159 * \tparam MatrixType type of \p arg, the argument of matrix square root,
160 * expected to be an instantiation of the Matrix class template.
161 * \tparam ResultType type of \p result, where result is to be stored.
162 * \param[in] arg argument of matrix square root.
163 * \param[out] result matrix square root of upper Hessenberg part of \p arg.
164 *
165 * This function computes the square root of the upper quasi-triangular matrix stored in the upper
166 * Hessenberg part of \p arg. Only the upper Hessenberg part of \p result is updated, the rest is
167 * not touched. See MatrixBase::sqrt() for details on how this computation is implemented.
168 *
169 * \sa MatrixSquareRoot, MatrixSquareRootQuasiTriangular
170 */
171template <typename MatrixType, typename ResultType>
172void matrix_sqrt_quasi_triangular(const MatrixType& arg, ResultType& result) {
173 eigen_assert(arg.rows() == arg.cols());
174 result.resize(arg.rows(), arg.cols());
177}
178
179/** \ingroup MatrixFunctions_Module
180 * \brief Compute matrix square root of triangular matrix.
181 *
182 * \tparam MatrixType type of \p arg, the argument of matrix square root,
183 * expected to be an instantiation of the Matrix class template.
184 * \tparam ResultType type of \p result, where result is to be stored.
185 * \param[in] arg argument of matrix square root.
186 * \param[out] result matrix square root of upper triangular part of \p arg.
187 *
188 * Only the upper triangular part (including the diagonal) of \p result is updated, the rest is not
189 * touched. See MatrixBase::sqrt() for details on how this computation is implemented.
190 *
191 * \sa MatrixSquareRoot, MatrixSquareRootQuasiTriangular
192 */
193template <typename MatrixType, typename ResultType>
194void matrix_sqrt_triangular(const MatrixType& arg, ResultType& result) {
195 using std::sqrt;
196 typedef typename MatrixType::Scalar Scalar;
197
198 eigen_assert(arg.rows() == arg.cols());
199
200 // Compute square root of arg and store it in upper triangular part of result
201 // This uses that the square root of triangular matrices can be computed directly.
202 result.resize(arg.rows(), arg.cols());
203 for (Index i = 0; i < arg.rows(); i++) {
204 result.coeffRef(i, i) = sqrt(arg.coeff(i, i));
205 }
206 for (Index j = 1; j < arg.cols(); j++) {
207 for (Index i = j - 1; i >= 0; i--) {
208 // if i = j-1, then segment has length 0 so tmp = 0
209 Scalar tmp = (result.row(i).segment(i + 1, j - i - 1) * result.col(j).segment(i + 1, j - i - 1)).value();
210 // denominator may be zero if original matrix is singular
211 result.coeffRef(i, j) = (arg.coeff(i, j) - tmp) / (result.coeff(i, i) + result.coeff(j, j));
212 }
213 }
214}
215
216namespace internal {
217
218/** \ingroup MatrixFunctions_Module
219 * \brief Helper struct for computing matrix square roots of general matrices.
220 * \tparam MatrixType type of the argument of the matrix square root,
221 * expected to be an instantiation of the Matrix class template.
222 *
223 * \sa MatrixSquareRootTriangular, MatrixSquareRootQuasiTriangular, MatrixBase::sqrt()
224 */
225template <typename MatrixType, int IsComplex = NumTraits<typename internal::traits<MatrixType>::Scalar>::IsComplex>
227 /** \brief Compute the matrix square root
228 *
229 * \param[in] arg matrix whose square root is to be computed.
230 * \param[out] result square root of \p arg.
231 *
232 * See MatrixBase::sqrt() for details on how this computation is implemented.
233 */
234 template <typename ResultType>
235 static void run(const MatrixType& arg, ResultType& result);
236};
237
238// ********** Partial specialization for real matrices **********
239
240template <typename MatrixType>
241struct matrix_sqrt_compute<MatrixType, 0> {
242 typedef typename MatrixType::PlainObject PlainType;
243 template <typename ResultType>
244 static void run(const MatrixType& arg, ResultType& result) {
245 eigen_assert(arg.rows() == arg.cols());
246
247 // Compute Schur decomposition of arg
248 const RealSchur<PlainType> schurOfA(arg);
249 const PlainType& T = schurOfA.matrixT();
250 const PlainType& U = schurOfA.matrixU();
251
252 // Compute square root of T
253 PlainType sqrtT = PlainType::Zero(arg.rows(), arg.cols());
255
256 // Compute square root of arg
257 result = U * sqrtT * U.adjoint();
258 }
259};
260
261// ********** Partial specialization for complex matrices **********
262
263template <typename MatrixType>
264struct matrix_sqrt_compute<MatrixType, 1> {
265 typedef typename MatrixType::PlainObject PlainType;
266 template <typename ResultType>
267 static void run(const MatrixType& arg, ResultType& result) {
268 eigen_assert(arg.rows() == arg.cols());
269
270 // Compute Schur decomposition of arg
271 const ComplexSchur<PlainType> schurOfA(arg);
272 const PlainType& T = schurOfA.matrixT();
273 const PlainType& U = schurOfA.matrixU();
274
275 // Compute square root of T
276 PlainType sqrtT;
277 matrix_sqrt_triangular(T, sqrtT);
278
279 // Compute square root of arg
280 result = U * (sqrtT.template triangularView<Upper>() * U.adjoint());
281 }
282};
283
284} // end namespace internal
285
286/** \ingroup MatrixFunctions_Module
287 *
288 * \brief Proxy for the matrix square root of some matrix (expression).
289 *
290 * \tparam Derived Type of the argument to the matrix square root.
291 *
292 * This class holds the argument to the matrix square root until it
293 * is assigned or evaluated for some other reason (so the argument
294 * should not be changed in the meantime). It is the return type of
295 * MatrixBase::sqrt() and most of the time this is the only way it is
296 * used.
297 */
298template <typename Derived>
299class MatrixSquareRootReturnValue : public ReturnByValue<MatrixSquareRootReturnValue<Derived> > {
300 protected:
302
303 public:
304 /** \brief Constructor.
305 *
306 * \param[in] src %Matrix (expression) forming the argument of the
307 * matrix square root.
308 */
309 explicit MatrixSquareRootReturnValue(const Derived& src) : m_src(src) {}
310
311 /** \brief Compute the matrix square root.
312 *
313 * \param[out] result the matrix square root of \p src in the
314 * constructor.
315 */
316 template <typename ResultType>
317 inline void evalTo(ResultType& result) const {
318 typedef typename internal::nested_eval<Derived, 10>::type DerivedEvalType;
319 typedef internal::remove_all_t<DerivedEvalType> DerivedEvalTypeClean;
320 DerivedEvalType tmp(m_src);
322 }
323
324 Index rows() const { return m_src.rows(); }
325 Index cols() const { return m_src.cols(); }
326
327 protected:
329};
330
331namespace internal {
332template <typename Derived>
333struct traits<MatrixSquareRootReturnValue<Derived> > {
334 typedef typename Derived::PlainObject ReturnType;
335};
336} // namespace internal
337
338template <typename Derived>
340 eigen_assert(rows() == cols());
341 return MatrixSquareRootReturnValue<Derived>(derived());
342}
343
344} // end namespace Eigen
345
346#endif // EIGEN_MATRIX_FUNCTION
Proxy for the matrix square root of some matrix (expression).
Definition: MatrixSquareRoot.h:299
internal::ref_selector< Derived >::type DerivedNested
Definition: MatrixSquareRoot.h:301
Index cols() const
Definition: MatrixSquareRoot.h:325
const DerivedNested m_src
Definition: MatrixSquareRoot.h:328
void evalTo(ResultType &result) const
Compute the matrix square root.
Definition: MatrixSquareRoot.h:317
MatrixSquareRootReturnValue(const Derived &src)
Constructor.
Definition: MatrixSquareRoot.h:309
Index rows() const
Definition: MatrixSquareRoot.h:324
auto arg(const Char *name, const T &arg) -> detail::named_arg< Char, T >
\rst Returns a named argument to be used in a formatting function.
Definition: core.h:1841
void matrix_sqrt_quasi_triangular(const MatrixType &arg, ResultType &result)
Compute matrix square root of quasi-triangular matrix.
Definition: MatrixSquareRoot.h:172
void matrix_sqrt_triangular(const MatrixType &arg, ResultType &result)
Compute matrix square root of triangular matrix.
Definition: MatrixSquareRoot.h:194
auto sqrt(const UnitType &value) noexcept -> unit_t< square_root< typename units::traits::unit_t_traits< UnitType >::unit_type >, typename units::traits::unit_t_traits< UnitType >::underlying_type, linear_scale >
computes the square root of value
Definition: math.h:483
void matrix_sqrt_quasi_triangular_1x2_off_diagonal_block(const MatrixType &T, Index i, Index j, ResultType &sqrtT)
Definition: MatrixSquareRoot.h:45
void matrix_sqrt_quasi_triangular_solve_auxiliary_equation(MatrixType &X, const MatrixType &A, const MatrixType &B, const MatrixType &C)
Definition: MatrixSquareRoot.h:67
void matrix_sqrt_quasi_triangular_diagonal(const MatrixType &T, ResultType &sqrtT)
Definition: MatrixSquareRoot.h:115
void matrix_sqrt_quasi_triangular_2x2_diagonal_block(const MatrixType &T, Index i, ResultType &sqrtT)
Definition: MatrixSquareRoot.h:23
void matrix_sqrt_quasi_triangular_off_diagonal(const MatrixType &T, ResultType &sqrtT)
Definition: MatrixSquareRoot.h:132
void matrix_sqrt_quasi_triangular_2x2_off_diagonal_block(const MatrixType &T, Index i, Index j, ResultType &sqrtT)
Definition: MatrixSquareRoot.h:101
void matrix_sqrt_quasi_triangular_1x1_off_diagonal_block(const MatrixType &T, Index i, Index j, ResultType &sqrtT)
Definition: MatrixSquareRoot.h:37
void matrix_sqrt_quasi_triangular_2x1_off_diagonal_block(const MatrixType &T, Index i, Index j, ResultType &sqrtT)
Definition: MatrixSquareRoot.h:56
Definition: MatrixSquareRoot.h:16
type
Definition: core.h:556
MatrixType::PlainObject PlainType
Definition: MatrixSquareRoot.h:242
static void run(const MatrixType &arg, ResultType &result)
Definition: MatrixSquareRoot.h:244
static void run(const MatrixType &arg, ResultType &result)
Definition: MatrixSquareRoot.h:267
MatrixType::PlainObject PlainType
Definition: MatrixSquareRoot.h:265
Helper struct for computing matrix square roots of general matrices.
Definition: MatrixSquareRoot.h:226
static void run(const MatrixType &arg, ResultType &result)
Compute the matrix square root.
Derived::PlainObject ReturnType
Definition: MatrixSquareRoot.h:334