mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-23 01:21:42 +00:00
[wpimath] Move Eigen unsupported folder into eigeninclude
This fixes relative includes in development versions of Eigen.
This commit is contained in:
Tyler Veness
committed by
Peter Johnson
Peter Johnson
parent
224f3a05cf
commit
f1e64b349a
@@ -1,40 +0,0 @@
|
||||
// This file is part of Eigen, a lightweight C++ template library
|
||||
// for linear algebra.
|
||||
//
|
||||
// Copyright (C) 2008-2009 Gael Guennebaud <g.gael@free.fr>
|
||||
//
|
||||
// This Source Code Form is subject to the terms of the Mozilla
|
||||
// Public License v. 2.0. If a copy of the MPL was not distributed
|
||||
// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
|
||||
|
||||
#ifndef EIGEN_AUTODIFF_MODULE
|
||||
#define EIGEN_AUTODIFF_MODULE
|
||||
|
||||
namespace Eigen {
|
||||
|
||||
/**
|
||||
* \defgroup AutoDiff_Module Auto Diff module
|
||||
*
|
||||
* This module features forward automatic differentation via a simple
|
||||
* templated scalar type wrapper AutoDiffScalar.
|
||||
*
|
||||
* Warning : this should NOT be confused with numerical differentiation, which
|
||||
* is a different method and has its own module in Eigen : \ref NumericalDiff_Module.
|
||||
*
|
||||
* \code
|
||||
* #include <unsupported/Eigen/AutoDiff>
|
||||
* \endcode
|
||||
*/
|
||||
//@{
|
||||
|
||||
}
|
||||
|
||||
#include "src/AutoDiff/AutoDiffScalar.h"
|
||||
// #include "src/AutoDiff/AutoDiffVector.h"
|
||||
#include "src/AutoDiff/AutoDiffJacobian.h"
|
||||
|
||||
namespace Eigen {
|
||||
//@}
|
||||
}
|
||||
|
||||
#endif // EIGEN_AUTODIFF_MODULE
|
||||
@@ -1,500 +0,0 @@
|
||||
// This file is part of Eigen, a lightweight C++ template library
|
||||
// for linear algebra.
|
||||
//
|
||||
// Copyright (C) 2009 Jitse Niesen <jitse@maths.leeds.ac.uk>
|
||||
// Copyright (C) 2012 Chen-Pang He <jdh8@ms63.hinet.net>
|
||||
//
|
||||
// This Source Code Form is subject to the terms of the Mozilla
|
||||
// Public License v. 2.0. If a copy of the MPL was not distributed
|
||||
// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
|
||||
|
||||
#ifndef EIGEN_MATRIX_FUNCTIONS
|
||||
#define EIGEN_MATRIX_FUNCTIONS
|
||||
|
||||
#include <cfloat>
|
||||
#include <list>
|
||||
|
||||
#include <Eigen/Core>
|
||||
#include <Eigen/LU>
|
||||
#include <Eigen/Eigenvalues>
|
||||
|
||||
/**
|
||||
* \defgroup MatrixFunctions_Module Matrix functions module
|
||||
* \brief This module aims to provide various methods for the computation of
|
||||
* matrix functions.
|
||||
*
|
||||
* To use this module, add
|
||||
* \code
|
||||
* #include <unsupported/Eigen/MatrixFunctions>
|
||||
* \endcode
|
||||
* at the start of your source file.
|
||||
*
|
||||
* This module defines the following MatrixBase methods.
|
||||
* - \ref matrixbase_cos "MatrixBase::cos()", for computing the matrix cosine
|
||||
* - \ref matrixbase_cosh "MatrixBase::cosh()", for computing the matrix hyperbolic cosine
|
||||
* - \ref matrixbase_exp "MatrixBase::exp()", for computing the matrix exponential
|
||||
* - \ref matrixbase_log "MatrixBase::log()", for computing the matrix logarithm
|
||||
* - \ref matrixbase_pow "MatrixBase::pow()", for computing the matrix power
|
||||
* - \ref matrixbase_matrixfunction "MatrixBase::matrixFunction()", for computing general matrix functions
|
||||
* - \ref matrixbase_sin "MatrixBase::sin()", for computing the matrix sine
|
||||
* - \ref matrixbase_sinh "MatrixBase::sinh()", for computing the matrix hyperbolic sine
|
||||
* - \ref matrixbase_sqrt "MatrixBase::sqrt()", for computing the matrix square root
|
||||
*
|
||||
* These methods are the main entry points to this module.
|
||||
*
|
||||
* %Matrix functions are defined as follows. Suppose that \f$ f \f$
|
||||
* is an entire function (that is, a function on the complex plane
|
||||
* that is everywhere complex differentiable). Then its Taylor
|
||||
* series
|
||||
* \f[ f(0) + f'(0) x + \frac{f''(0)}{2} x^2 + \frac{f'''(0)}{3!} x^3 + \cdots \f]
|
||||
* converges to \f$ f(x) \f$. In this case, we can define the matrix
|
||||
* function by the same series:
|
||||
* \f[ f(M) = f(0) + f'(0) M + \frac{f''(0)}{2} M^2 + \frac{f'''(0)}{3!} M^3 + \cdots \f]
|
||||
*
|
||||
*/
|
||||
|
||||
#include "src/MatrixFunctions/MatrixExponential.h"
|
||||
#include "src/MatrixFunctions/MatrixFunction.h"
|
||||
#include "src/MatrixFunctions/MatrixSquareRoot.h"
|
||||
#include "src/MatrixFunctions/MatrixLogarithm.h"
|
||||
#include "src/MatrixFunctions/MatrixPower.h"
|
||||
|
||||
|
||||
/**
|
||||
\page matrixbaseextra_page
|
||||
\ingroup MatrixFunctions_Module
|
||||
|
||||
\section matrixbaseextra MatrixBase methods defined in the MatrixFunctions module
|
||||
|
||||
The remainder of the page documents the following MatrixBase methods
|
||||
which are defined in the MatrixFunctions module.
|
||||
|
||||
|
||||
|
||||
\subsection matrixbase_cos MatrixBase::cos()
|
||||
|
||||
Compute the matrix cosine.
|
||||
|
||||
\code
|
||||
const MatrixFunctionReturnValue<Derived> MatrixBase<Derived>::cos() const
|
||||
\endcode
|
||||
|
||||
\param[in] M a square matrix.
|
||||
\returns expression representing \f$ \cos(M) \f$.
|
||||
|
||||
This function computes the matrix cosine. Use ArrayBase::cos() for computing the entry-wise cosine.
|
||||
|
||||
The implementation calls \ref matrixbase_matrixfunction "matrixFunction()" with StdStemFunctions::cos().
|
||||
|
||||
\sa \ref matrixbase_sin "sin()" for an example.
|
||||
|
||||
|
||||
|
||||
\subsection matrixbase_cosh MatrixBase::cosh()
|
||||
|
||||
Compute the matrix hyberbolic cosine.
|
||||
|
||||
\code
|
||||
const MatrixFunctionReturnValue<Derived> MatrixBase<Derived>::cosh() const
|
||||
\endcode
|
||||
|
||||
\param[in] M a square matrix.
|
||||
\returns expression representing \f$ \cosh(M) \f$
|
||||
|
||||
This function calls \ref matrixbase_matrixfunction "matrixFunction()" with StdStemFunctions::cosh().
|
||||
|
||||
\sa \ref matrixbase_sinh "sinh()" for an example.
|
||||
|
||||
|
||||
|
||||
\subsection matrixbase_exp MatrixBase::exp()
|
||||
|
||||
Compute the matrix exponential.
|
||||
|
||||
\code
|
||||
const MatrixExponentialReturnValue<Derived> MatrixBase<Derived>::exp() const
|
||||
\endcode
|
||||
|
||||
\param[in] M matrix whose exponential is to be computed.
|
||||
\returns expression representing the matrix exponential of \p M.
|
||||
|
||||
The matrix exponential of \f$ M \f$ is defined by
|
||||
\f[ \exp(M) = \sum_{k=0}^\infty \frac{M^k}{k!}. \f]
|
||||
The matrix exponential can be used to solve linear ordinary
|
||||
differential equations: the solution of \f$ y' = My \f$ with the
|
||||
initial condition \f$ y(0) = y_0 \f$ is given by
|
||||
\f$ y(t) = \exp(M) y_0 \f$.
|
||||
|
||||
The matrix exponential is different from applying the exp function to all the entries in the matrix.
|
||||
Use ArrayBase::exp() if you want to do the latter.
|
||||
|
||||
The cost of the computation is approximately \f$ 20 n^3 \f$ for
|
||||
matrices of size \f$ n \f$. The number 20 depends weakly on the
|
||||
norm of the matrix.
|
||||
|
||||
The matrix exponential is computed using the scaling-and-squaring
|
||||
method combined with Padé approximation. The matrix is first
|
||||
rescaled, then the exponential of the reduced matrix is computed
|
||||
approximant, and then the rescaling is undone by repeated
|
||||
squaring. The degree of the Padé approximant is chosen such
|
||||
that the approximation error is less than the round-off
|
||||
error. However, errors may accumulate during the squaring phase.
|
||||
|
||||
Details of the algorithm can be found in: Nicholas J. Higham, "The
|
||||
scaling and squaring method for the matrix exponential revisited,"
|
||||
<em>SIAM J. %Matrix Anal. Applic.</em>, <b>26</b>:1179–1193,
|
||||
2005.
|
||||
|
||||
Example: The following program checks that
|
||||
\f[ \exp \left[ \begin{array}{ccc}
|
||||
0 & \frac14\pi & 0 \\
|
||||
-\frac14\pi & 0 & 0 \\
|
||||
0 & 0 & 0
|
||||
\end{array} \right] = \left[ \begin{array}{ccc}
|
||||
\frac12\sqrt2 & -\frac12\sqrt2 & 0 \\
|
||||
\frac12\sqrt2 & \frac12\sqrt2 & 0 \\
|
||||
0 & 0 & 1
|
||||
\end{array} \right]. \f]
|
||||
This corresponds to a rotation of \f$ \frac14\pi \f$ radians around
|
||||
the z-axis.
|
||||
|
||||
\include MatrixExponential.cpp
|
||||
Output: \verbinclude MatrixExponential.out
|
||||
|
||||
\note \p M has to be a matrix of \c float, \c double, `long double`
|
||||
\c complex<float>, \c complex<double>, or `complex<long double>` .
|
||||
|
||||
|
||||
\subsection matrixbase_log MatrixBase::log()
|
||||
|
||||
Compute the matrix logarithm.
|
||||
|
||||
\code
|
||||
const MatrixLogarithmReturnValue<Derived> MatrixBase<Derived>::log() const
|
||||
\endcode
|
||||
|
||||
\param[in] M invertible matrix whose logarithm is to be computed.
|
||||
\returns expression representing the matrix logarithm root of \p M.
|
||||
|
||||
The matrix logarithm of \f$ M \f$ is a matrix \f$ X \f$ such that
|
||||
\f$ \exp(X) = M \f$ where exp denotes the matrix exponential. As for
|
||||
the scalar logarithm, the equation \f$ \exp(X) = M \f$ may have
|
||||
multiple solutions; this function returns a matrix whose eigenvalues
|
||||
have imaginary part in the interval \f$ (-\pi,\pi] \f$.
|
||||
|
||||
The matrix logarithm is different from applying the log function to all the entries in the matrix.
|
||||
Use ArrayBase::log() if you want to do the latter.
|
||||
|
||||
In the real case, the matrix \f$ M \f$ should be invertible and
|
||||
it should have no eigenvalues which are real and negative (pairs of
|
||||
complex conjugate eigenvalues are allowed). In the complex case, it
|
||||
only needs to be invertible.
|
||||
|
||||
This function computes the matrix logarithm using the Schur-Parlett
|
||||
algorithm as implemented by MatrixBase::matrixFunction(). The
|
||||
logarithm of an atomic block is computed by MatrixLogarithmAtomic,
|
||||
which uses direct computation for 1-by-1 and 2-by-2 blocks and an
|
||||
inverse scaling-and-squaring algorithm for bigger blocks, with the
|
||||
square roots computed by MatrixBase::sqrt().
|
||||
|
||||
Details of the algorithm can be found in Section 11.6.2 of:
|
||||
Nicholas J. Higham,
|
||||
<em>Functions of Matrices: Theory and Computation</em>,
|
||||
SIAM 2008. ISBN 978-0-898716-46-7.
|
||||
|
||||
Example: The following program checks that
|
||||
\f[ \log \left[ \begin{array}{ccc}
|
||||
\frac12\sqrt2 & -\frac12\sqrt2 & 0 \\
|
||||
\frac12\sqrt2 & \frac12\sqrt2 & 0 \\
|
||||
0 & 0 & 1
|
||||
\end{array} \right] = \left[ \begin{array}{ccc}
|
||||
0 & \frac14\pi & 0 \\
|
||||
-\frac14\pi & 0 & 0 \\
|
||||
0 & 0 & 0
|
||||
\end{array} \right]. \f]
|
||||
This corresponds to a rotation of \f$ \frac14\pi \f$ radians around
|
||||
the z-axis. This is the inverse of the example used in the
|
||||
documentation of \ref matrixbase_exp "exp()".
|
||||
|
||||
\include MatrixLogarithm.cpp
|
||||
Output: \verbinclude MatrixLogarithm.out
|
||||
|
||||
\note \p M has to be a matrix of \c float, \c double, `long
|
||||
double`, \c complex<float>, \c complex<double>, or `complex<long double>`.
|
||||
|
||||
\sa MatrixBase::exp(), MatrixBase::matrixFunction(),
|
||||
class MatrixLogarithmAtomic, MatrixBase::sqrt().
|
||||
|
||||
|
||||
\subsection matrixbase_pow MatrixBase::pow()
|
||||
|
||||
Compute the matrix raised to arbitrary real power.
|
||||
|
||||
\code
|
||||
const MatrixPowerReturnValue<Derived> MatrixBase<Derived>::pow(RealScalar p) const
|
||||
\endcode
|
||||
|
||||
\param[in] M base of the matrix power, should be a square matrix.
|
||||
\param[in] p exponent of the matrix power.
|
||||
|
||||
The matrix power \f$ M^p \f$ is defined as \f$ \exp(p \log(M)) \f$,
|
||||
where exp denotes the matrix exponential, and log denotes the matrix
|
||||
logarithm. This is different from raising all the entries in the matrix
|
||||
to the p-th power. Use ArrayBase::pow() if you want to do the latter.
|
||||
|
||||
If \p p is complex, the scalar type of \p M should be the type of \p
|
||||
p . \f$ M^p \f$ simply evaluates into \f$ \exp(p \log(M)) \f$.
|
||||
Therefore, the matrix \f$ M \f$ should meet the conditions to be an
|
||||
argument of matrix logarithm.
|
||||
|
||||
If \p p is real, it is casted into the real scalar type of \p M. Then
|
||||
this function computes the matrix power using the Schur-Padé
|
||||
algorithm as implemented by class MatrixPower. The exponent is split
|
||||
into integral part and fractional part, where the fractional part is
|
||||
in the interval \f$ (-1, 1) \f$. The main diagonal and the first
|
||||
super-diagonal is directly computed.
|
||||
|
||||
If \p M is singular with a semisimple zero eigenvalue and \p p is
|
||||
positive, the Schur factor \f$ T \f$ is reordered with Givens
|
||||
rotations, i.e.
|
||||
|
||||
\f[ T = \left[ \begin{array}{cc}
|
||||
T_1 & T_2 \\
|
||||
0 & 0
|
||||
\end{array} \right] \f]
|
||||
|
||||
where \f$ T_1 \f$ is invertible. Then \f$ T^p \f$ is given by
|
||||
|
||||
\f[ T^p = \left[ \begin{array}{cc}
|
||||
T_1^p & T_1^{-1} T_1^p T_2 \\
|
||||
0 & 0
|
||||
\end{array}. \right] \f]
|
||||
|
||||
\warning Fractional power of a matrix with a non-semisimple zero
|
||||
eigenvalue is not well-defined. We introduce an assertion failure
|
||||
against inaccurate result, e.g. \code
|
||||
#include <unsupported/Eigen/MatrixFunctions>
|
||||
#include <iostream>
|
||||
|
||||
int main()
|
||||
{
|
||||
Eigen::Matrix4d A;
|
||||
A << 0, 0, 2, 3,
|
||||
0, 0, 4, 5,
|
||||
0, 0, 6, 7,
|
||||
0, 0, 8, 9;
|
||||
std::cout << A.pow(0.37) << std::endl;
|
||||
|
||||
// The 1 makes eigenvalue 0 non-semisimple.
|
||||
A.coeffRef(0, 1) = 1;
|
||||
|
||||
// This fails if EIGEN_NO_DEBUG is undefined.
|
||||
std::cout << A.pow(0.37) << std::endl;
|
||||
|
||||
return 0;
|
||||
}
|
||||
\endcode
|
||||
|
||||
Details of the algorithm can be found in: Nicholas J. Higham and
|
||||
Lijing Lin, "A Schur-Padé algorithm for fractional powers of a
|
||||
matrix," <em>SIAM J. %Matrix Anal. Applic.</em>,
|
||||
<b>32(3)</b>:1056–1078, 2011.
|
||||
|
||||
Example: The following program checks that
|
||||
\f[ \left[ \begin{array}{ccc}
|
||||
\cos1 & -\sin1 & 0 \\
|
||||
\sin1 & \cos1 & 0 \\
|
||||
0 & 0 & 1
|
||||
\end{array} \right]^{\frac14\pi} = \left[ \begin{array}{ccc}
|
||||
\frac12\sqrt2 & -\frac12\sqrt2 & 0 \\
|
||||
\frac12\sqrt2 & \frac12\sqrt2 & 0 \\
|
||||
0 & 0 & 1
|
||||
\end{array} \right]. \f]
|
||||
This corresponds to \f$ \frac14\pi \f$ rotations of 1 radian around
|
||||
the z-axis.
|
||||
|
||||
\include MatrixPower.cpp
|
||||
Output: \verbinclude MatrixPower.out
|
||||
|
||||
MatrixBase::pow() is user-friendly. However, there are some
|
||||
circumstances under which you should use class MatrixPower directly.
|
||||
MatrixPower can save the result of Schur decomposition, so it's
|
||||
better for computing various powers for the same matrix.
|
||||
|
||||
Example:
|
||||
\include MatrixPower_optimal.cpp
|
||||
Output: \verbinclude MatrixPower_optimal.out
|
||||
|
||||
\note \p M has to be a matrix of \c float, \c double, `long
|
||||
double`, \c complex<float>, \c complex<double>, or
|
||||
\c complex<long double> .
|
||||
|
||||
\sa MatrixBase::exp(), MatrixBase::log(), class MatrixPower.
|
||||
|
||||
|
||||
\subsection matrixbase_matrixfunction MatrixBase::matrixFunction()
|
||||
|
||||
Compute a matrix function.
|
||||
|
||||
\code
|
||||
const MatrixFunctionReturnValue<Derived> MatrixBase<Derived>::matrixFunction(typename internal::stem_function<typename internal::traits<Derived>::Scalar>::type f) const
|
||||
\endcode
|
||||
|
||||
\param[in] M argument of matrix function, should be a square matrix.
|
||||
\param[in] f an entire function; \c f(x,n) should compute the n-th
|
||||
derivative of f at x.
|
||||
\returns expression representing \p f applied to \p M.
|
||||
|
||||
Suppose that \p M is a matrix whose entries have type \c Scalar.
|
||||
Then, the second argument, \p f, should be a function with prototype
|
||||
\code
|
||||
ComplexScalar f(ComplexScalar, int)
|
||||
\endcode
|
||||
where \c ComplexScalar = \c std::complex<Scalar> if \c Scalar is
|
||||
real (e.g., \c float or \c double) and \c ComplexScalar =
|
||||
\c Scalar if \c Scalar is complex. The return value of \c f(x,n)
|
||||
should be \f$ f^{(n)}(x) \f$, the n-th derivative of f at x.
|
||||
|
||||
This routine uses the algorithm described in:
|
||||
Philip Davies and Nicholas J. Higham,
|
||||
"A Schur-Parlett algorithm for computing matrix functions",
|
||||
<em>SIAM J. %Matrix Anal. Applic.</em>, <b>25</b>:464–485, 2003.
|
||||
|
||||
The actual work is done by the MatrixFunction class.
|
||||
|
||||
Example: The following program checks that
|
||||
\f[ \exp \left[ \begin{array}{ccc}
|
||||
0 & \frac14\pi & 0 \\
|
||||
-\frac14\pi & 0 & 0 \\
|
||||
0 & 0 & 0
|
||||
\end{array} \right] = \left[ \begin{array}{ccc}
|
||||
\frac12\sqrt2 & -\frac12\sqrt2 & 0 \\
|
||||
\frac12\sqrt2 & \frac12\sqrt2 & 0 \\
|
||||
0 & 0 & 1
|
||||
\end{array} \right]. \f]
|
||||
This corresponds to a rotation of \f$ \frac14\pi \f$ radians around
|
||||
the z-axis. This is the same example as used in the documentation
|
||||
of \ref matrixbase_exp "exp()".
|
||||
|
||||
\include MatrixFunction.cpp
|
||||
Output: \verbinclude MatrixFunction.out
|
||||
|
||||
Note that the function \c expfn is defined for complex numbers
|
||||
\c x, even though the matrix \c A is over the reals. Instead of
|
||||
\c expfn, we could also have used StdStemFunctions::exp:
|
||||
\code
|
||||
A.matrixFunction(StdStemFunctions<std::complex<double> >::exp, &B);
|
||||
\endcode
|
||||
|
||||
|
||||
|
||||
\subsection matrixbase_sin MatrixBase::sin()
|
||||
|
||||
Compute the matrix sine.
|
||||
|
||||
\code
|
||||
const MatrixFunctionReturnValue<Derived> MatrixBase<Derived>::sin() const
|
||||
\endcode
|
||||
|
||||
\param[in] M a square matrix.
|
||||
\returns expression representing \f$ \sin(M) \f$.
|
||||
|
||||
This function computes the matrix sine. Use ArrayBase::sin() for computing the entry-wise sine.
|
||||
|
||||
The implementation calls \ref matrixbase_matrixfunction "matrixFunction()" with StdStemFunctions::sin().
|
||||
|
||||
Example: \include MatrixSine.cpp
|
||||
Output: \verbinclude MatrixSine.out
|
||||
|
||||
|
||||
|
||||
\subsection matrixbase_sinh MatrixBase::sinh()
|
||||
|
||||
Compute the matrix hyperbolic sine.
|
||||
|
||||
\code
|
||||
MatrixFunctionReturnValue<Derived> MatrixBase<Derived>::sinh() const
|
||||
\endcode
|
||||
|
||||
\param[in] M a square matrix.
|
||||
\returns expression representing \f$ \sinh(M) \f$
|
||||
|
||||
This function calls \ref matrixbase_matrixfunction "matrixFunction()" with StdStemFunctions::sinh().
|
||||
|
||||
Example: \include MatrixSinh.cpp
|
||||
Output: \verbinclude MatrixSinh.out
|
||||
|
||||
|
||||
\subsection matrixbase_sqrt MatrixBase::sqrt()
|
||||
|
||||
Compute the matrix square root.
|
||||
|
||||
\code
|
||||
const MatrixSquareRootReturnValue<Derived> MatrixBase<Derived>::sqrt() const
|
||||
\endcode
|
||||
|
||||
\param[in] M invertible matrix whose square root is to be computed.
|
||||
\returns expression representing the matrix square root of \p M.
|
||||
|
||||
The matrix square root of \f$ M \f$ is the matrix \f$ M^{1/2} \f$
|
||||
whose square is the original matrix; so if \f$ S = M^{1/2} \f$ then
|
||||
\f$ S^2 = M \f$. This is different from taking the square root of all
|
||||
the entries in the matrix; use ArrayBase::sqrt() if you want to do the
|
||||
latter.
|
||||
|
||||
In the <b>real case</b>, the matrix \f$ M \f$ should be invertible and
|
||||
it should have no eigenvalues which are real and negative (pairs of
|
||||
complex conjugate eigenvalues are allowed). In that case, the matrix
|
||||
has a square root which is also real, and this is the square root
|
||||
computed by this function.
|
||||
|
||||
The matrix square root is computed by first reducing the matrix to
|
||||
quasi-triangular form with the real Schur decomposition. The square
|
||||
root of the quasi-triangular matrix can then be computed directly. The
|
||||
cost is approximately \f$ 25 n^3 \f$ real flops for the real Schur
|
||||
decomposition and \f$ 3\frac13 n^3 \f$ real flops for the remainder
|
||||
(though the computation time in practice is likely more than this
|
||||
indicates).
|
||||
|
||||
Details of the algorithm can be found in: Nicholas J. Highan,
|
||||
"Computing real square roots of a real matrix", <em>Linear Algebra
|
||||
Appl.</em>, 88/89:405–430, 1987.
|
||||
|
||||
If the matrix is <b>positive-definite symmetric</b>, then the square
|
||||
root is also positive-definite symmetric. In this case, it is best to
|
||||
use SelfAdjointEigenSolver::operatorSqrt() to compute it.
|
||||
|
||||
In the <b>complex case</b>, the matrix \f$ M \f$ should be invertible;
|
||||
this is a restriction of the algorithm. The square root computed by
|
||||
this algorithm is the one whose eigenvalues have an argument in the
|
||||
interval \f$ (-\frac12\pi, \frac12\pi] \f$. This is the usual branch
|
||||
cut.
|
||||
|
||||
The computation is the same as in the real case, except that the
|
||||
complex Schur decomposition is used to reduce the matrix to a
|
||||
triangular matrix. The theoretical cost is the same. Details are in:
|
||||
Åke Björck and Sven Hammarling, "A Schur method for the
|
||||
square root of a matrix", <em>Linear Algebra Appl.</em>,
|
||||
52/53:127–140, 1983.
|
||||
|
||||
Example: The following program checks that the square root of
|
||||
\f[ \left[ \begin{array}{cc}
|
||||
\cos(\frac13\pi) & -\sin(\frac13\pi) \\
|
||||
\sin(\frac13\pi) & \cos(\frac13\pi)
|
||||
\end{array} \right], \f]
|
||||
corresponding to a rotation over 60 degrees, is a rotation over 30 degrees:
|
||||
\f[ \left[ \begin{array}{cc}
|
||||
\cos(\frac16\pi) & -\sin(\frac16\pi) \\
|
||||
\sin(\frac16\pi) & \cos(\frac16\pi)
|
||||
\end{array} \right]. \f]
|
||||
|
||||
\include MatrixSquareRoot.cpp
|
||||
Output: \verbinclude MatrixSquareRoot.out
|
||||
|
||||
\sa class RealSchur, class ComplexSchur, class MatrixSquareRoot,
|
||||
SelfAdjointEigenSolver::operatorSqrt().
|
||||
|
||||
*/
|
||||
|
||||
#endif // EIGEN_MATRIX_FUNCTIONS
|
||||
|
||||
@@ -1,108 +0,0 @@
|
||||
// This file is part of Eigen, a lightweight C++ template library
|
||||
// for linear algebra.
|
||||
//
|
||||
// Copyright (C) 2009 Gael Guennebaud <gael.guennebaud@inria.fr>
|
||||
//
|
||||
// This Source Code Form is subject to the terms of the Mozilla
|
||||
// Public License v. 2.0. If a copy of the MPL was not distributed
|
||||
// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
|
||||
|
||||
#ifndef EIGEN_AUTODIFF_JACOBIAN_H
|
||||
#define EIGEN_AUTODIFF_JACOBIAN_H
|
||||
|
||||
namespace Eigen
|
||||
{
|
||||
|
||||
template<typename Functor> class AutoDiffJacobian : public Functor
|
||||
{
|
||||
public:
|
||||
AutoDiffJacobian() : Functor() {}
|
||||
AutoDiffJacobian(const Functor& f) : Functor(f) {}
|
||||
|
||||
// forward constructors
|
||||
#if EIGEN_HAS_VARIADIC_TEMPLATES
|
||||
template<typename... T>
|
||||
AutoDiffJacobian(const T& ...Values) : Functor(Values...) {}
|
||||
#else
|
||||
template<typename T0>
|
||||
AutoDiffJacobian(const T0& a0) : Functor(a0) {}
|
||||
template<typename T0, typename T1>
|
||||
AutoDiffJacobian(const T0& a0, const T1& a1) : Functor(a0, a1) {}
|
||||
template<typename T0, typename T1, typename T2>
|
||||
AutoDiffJacobian(const T0& a0, const T1& a1, const T2& a2) : Functor(a0, a1, a2) {}
|
||||
#endif
|
||||
|
||||
typedef typename Functor::InputType InputType;
|
||||
typedef typename Functor::ValueType ValueType;
|
||||
typedef typename ValueType::Scalar Scalar;
|
||||
|
||||
enum {
|
||||
InputsAtCompileTime = InputType::RowsAtCompileTime,
|
||||
ValuesAtCompileTime = ValueType::RowsAtCompileTime
|
||||
};
|
||||
|
||||
typedef Matrix<Scalar, ValuesAtCompileTime, InputsAtCompileTime> JacobianType;
|
||||
typedef typename JacobianType::Index Index;
|
||||
|
||||
typedef Matrix<Scalar, InputsAtCompileTime, 1> DerivativeType;
|
||||
typedef AutoDiffScalar<DerivativeType> ActiveScalar;
|
||||
|
||||
typedef Matrix<ActiveScalar, InputsAtCompileTime, 1> ActiveInput;
|
||||
typedef Matrix<ActiveScalar, ValuesAtCompileTime, 1> ActiveValue;
|
||||
|
||||
#if EIGEN_HAS_VARIADIC_TEMPLATES
|
||||
// Some compilers don't accept variadic parameters after a default parameter,
|
||||
// i.e., we can't just write _jac=0 but we need to overload operator():
|
||||
EIGEN_STRONG_INLINE
|
||||
void operator() (const InputType& x, ValueType* v) const
|
||||
{
|
||||
this->operator()(x, v, 0);
|
||||
}
|
||||
template<typename... ParamsType>
|
||||
void operator() (const InputType& x, ValueType* v, JacobianType* _jac,
|
||||
const ParamsType&... Params) const
|
||||
#else
|
||||
void operator() (const InputType& x, ValueType* v, JacobianType* _jac=0) const
|
||||
#endif
|
||||
{
|
||||
eigen_assert(v!=0);
|
||||
|
||||
if (!_jac)
|
||||
{
|
||||
#if EIGEN_HAS_VARIADIC_TEMPLATES
|
||||
Functor::operator()(x, v, Params...);
|
||||
#else
|
||||
Functor::operator()(x, v);
|
||||
#endif
|
||||
return;
|
||||
}
|
||||
|
||||
JacobianType& jac = *_jac;
|
||||
|
||||
ActiveInput ax = x.template cast<ActiveScalar>();
|
||||
ActiveValue av(jac.rows());
|
||||
|
||||
if(InputsAtCompileTime==Dynamic)
|
||||
for (Index j=0; j<jac.rows(); j++)
|
||||
av[j].derivatives().resize(x.rows());
|
||||
|
||||
for (Index i=0; i<jac.cols(); i++)
|
||||
ax[i].derivatives() = DerivativeType::Unit(x.rows(),i);
|
||||
|
||||
#if EIGEN_HAS_VARIADIC_TEMPLATES
|
||||
Functor::operator()(ax, &av, Params...);
|
||||
#else
|
||||
Functor::operator()(ax, &av);
|
||||
#endif
|
||||
|
||||
for (Index i=0; i<jac.rows(); i++)
|
||||
{
|
||||
(*v)[i] = av[i].value();
|
||||
jac.row(i) = av[i].derivatives();
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
#endif // EIGEN_AUTODIFF_JACOBIAN_H
|
||||
@@ -1,720 +0,0 @@
|
||||
// This file is part of Eigen, a lightweight C++ template library
|
||||
// for linear algebra.
|
||||
//
|
||||
// Copyright (C) 2009 Gael Guennebaud <gael.guennebaud@inria.fr>
|
||||
//
|
||||
// This Source Code Form is subject to the terms of the Mozilla
|
||||
// Public License v. 2.0. If a copy of the MPL was not distributed
|
||||
// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
|
||||
|
||||
#ifndef EIGEN_AUTODIFF_SCALAR_H
|
||||
#define EIGEN_AUTODIFF_SCALAR_H
|
||||
|
||||
namespace Eigen {
|
||||
|
||||
namespace internal {
|
||||
|
||||
template<typename A, typename B>
|
||||
struct make_coherent_impl {
|
||||
static void run(A&, B&) {}
|
||||
};
|
||||
|
||||
// resize a to match b is a.size()==0, and conversely.
|
||||
template<typename A, typename B>
|
||||
void make_coherent(const A& a, const B&b)
|
||||
{
|
||||
make_coherent_impl<A,B>::run(a.const_cast_derived(), b.const_cast_derived());
|
||||
}
|
||||
|
||||
template<typename _DerType, bool Enable> struct auto_diff_special_op;
|
||||
|
||||
} // end namespace internal
|
||||
|
||||
template<typename _DerType> class AutoDiffScalar;
|
||||
|
||||
template<typename NewDerType>
|
||||
inline AutoDiffScalar<NewDerType> MakeAutoDiffScalar(const typename NewDerType::Scalar& value, const NewDerType &der) {
|
||||
return AutoDiffScalar<NewDerType>(value,der);
|
||||
}
|
||||
|
||||
/** \class AutoDiffScalar
|
||||
* \brief A scalar type replacement with automatic differentation capability
|
||||
*
|
||||
* \param _DerType the vector type used to store/represent the derivatives. The base scalar type
|
||||
* as well as the number of derivatives to compute are determined from this type.
|
||||
* Typical choices include, e.g., \c Vector4f for 4 derivatives, or \c VectorXf
|
||||
* if the number of derivatives is not known at compile time, and/or, the number
|
||||
* of derivatives is large.
|
||||
* Note that _DerType can also be a reference (e.g., \c VectorXf&) to wrap a
|
||||
* existing vector into an AutoDiffScalar.
|
||||
* Finally, _DerType can also be any Eigen compatible expression.
|
||||
*
|
||||
* This class represents a scalar value while tracking its respective derivatives using Eigen's expression
|
||||
* template mechanism.
|
||||
*
|
||||
* It supports the following list of global math function:
|
||||
* - std::abs, std::sqrt, std::pow, std::exp, std::log, std::sin, std::cos,
|
||||
* - internal::abs, internal::sqrt, numext::pow, internal::exp, internal::log, internal::sin, internal::cos,
|
||||
* - internal::conj, internal::real, internal::imag, numext::abs2.
|
||||
*
|
||||
* AutoDiffScalar can be used as the scalar type of an Eigen::Matrix object. However,
|
||||
* in that case, the expression template mechanism only occurs at the top Matrix level,
|
||||
* while derivatives are computed right away.
|
||||
*
|
||||
*/
|
||||
|
||||
template<typename _DerType>
|
||||
class AutoDiffScalar
|
||||
: public internal::auto_diff_special_op
|
||||
<_DerType, !internal::is_same<typename internal::traits<typename internal::remove_all<_DerType>::type>::Scalar,
|
||||
typename NumTraits<typename internal::traits<typename internal::remove_all<_DerType>::type>::Scalar>::Real>::value>
|
||||
{
|
||||
public:
|
||||
typedef internal::auto_diff_special_op
|
||||
<_DerType, !internal::is_same<typename internal::traits<typename internal::remove_all<_DerType>::type>::Scalar,
|
||||
typename NumTraits<typename internal::traits<typename internal::remove_all<_DerType>::type>::Scalar>::Real>::value> Base;
|
||||
typedef typename internal::remove_all<_DerType>::type DerType;
|
||||
typedef typename internal::traits<DerType>::Scalar Scalar;
|
||||
typedef typename NumTraits<Scalar>::Real Real;
|
||||
|
||||
using Base::operator+;
|
||||
using Base::operator*;
|
||||
|
||||
/** Default constructor without any initialization. */
|
||||
AutoDiffScalar() {}
|
||||
|
||||
/** Constructs an active scalar from its \a value,
|
||||
and initializes the \a nbDer derivatives such that it corresponds to the \a derNumber -th variable */
|
||||
AutoDiffScalar(const Scalar& value, int nbDer, int derNumber)
|
||||
: m_value(value), m_derivatives(DerType::Zero(nbDer))
|
||||
{
|
||||
m_derivatives.coeffRef(derNumber) = Scalar(1);
|
||||
}
|
||||
|
||||
/** Conversion from a scalar constant to an active scalar.
|
||||
* The derivatives are set to zero. */
|
||||
/*explicit*/ AutoDiffScalar(const Real& value)
|
||||
: m_value(value)
|
||||
{
|
||||
if(m_derivatives.size()>0)
|
||||
m_derivatives.setZero();
|
||||
}
|
||||
|
||||
/** Constructs an active scalar from its \a value and derivatives \a der */
|
||||
AutoDiffScalar(const Scalar& value, const DerType& der)
|
||||
: m_value(value), m_derivatives(der)
|
||||
{}
|
||||
|
||||
template<typename OtherDerType>
|
||||
AutoDiffScalar(const AutoDiffScalar<OtherDerType>& other
|
||||
#ifndef EIGEN_PARSED_BY_DOXYGEN
|
||||
, typename internal::enable_if<
|
||||
internal::is_same<Scalar, typename internal::traits<typename internal::remove_all<OtherDerType>::type>::Scalar>::value
|
||||
&& internal::is_convertible<OtherDerType,DerType>::value , void*>::type = 0
|
||||
#endif
|
||||
)
|
||||
: m_value(other.value()), m_derivatives(other.derivatives())
|
||||
{}
|
||||
|
||||
friend std::ostream & operator << (std::ostream & s, const AutoDiffScalar& a)
|
||||
{
|
||||
return s << a.value();
|
||||
}
|
||||
|
||||
AutoDiffScalar(const AutoDiffScalar& other)
|
||||
: m_value(other.value()), m_derivatives(other.derivatives())
|
||||
{}
|
||||
|
||||
template<typename OtherDerType>
|
||||
inline AutoDiffScalar& operator=(const AutoDiffScalar<OtherDerType>& other)
|
||||
{
|
||||
m_value = other.value();
|
||||
m_derivatives = other.derivatives();
|
||||
return *this;
|
||||
}
|
||||
|
||||
inline AutoDiffScalar& operator=(const AutoDiffScalar& other)
|
||||
{
|
||||
m_value = other.value();
|
||||
m_derivatives = other.derivatives();
|
||||
return *this;
|
||||
}
|
||||
|
||||
inline AutoDiffScalar& operator=(const Scalar& other)
|
||||
{
|
||||
m_value = other;
|
||||
if(m_derivatives.size()>0)
|
||||
m_derivatives.setZero();
|
||||
return *this;
|
||||
}
|
||||
|
||||
// inline operator const Scalar& () const { return m_value; }
|
||||
// inline operator Scalar& () { return m_value; }
|
||||
|
||||
inline const Scalar& value() const { return m_value; }
|
||||
inline Scalar& value() { return m_value; }
|
||||
|
||||
inline const DerType& derivatives() const { return m_derivatives; }
|
||||
inline DerType& derivatives() { return m_derivatives; }
|
||||
|
||||
inline bool operator< (const Scalar& other) const { return m_value < other; }
|
||||
inline bool operator<=(const Scalar& other) const { return m_value <= other; }
|
||||
inline bool operator> (const Scalar& other) const { return m_value > other; }
|
||||
inline bool operator>=(const Scalar& other) const { return m_value >= other; }
|
||||
inline bool operator==(const Scalar& other) const { return m_value == other; }
|
||||
inline bool operator!=(const Scalar& other) const { return m_value != other; }
|
||||
|
||||
friend inline bool operator< (const Scalar& a, const AutoDiffScalar& b) { return a < b.value(); }
|
||||
friend inline bool operator<=(const Scalar& a, const AutoDiffScalar& b) { return a <= b.value(); }
|
||||
friend inline bool operator> (const Scalar& a, const AutoDiffScalar& b) { return a > b.value(); }
|
||||
friend inline bool operator>=(const Scalar& a, const AutoDiffScalar& b) { return a >= b.value(); }
|
||||
friend inline bool operator==(const Scalar& a, const AutoDiffScalar& b) { return a == b.value(); }
|
||||
friend inline bool operator!=(const Scalar& a, const AutoDiffScalar& b) { return a != b.value(); }
|
||||
|
||||
template<typename OtherDerType> inline bool operator< (const AutoDiffScalar<OtherDerType>& b) const { return m_value < b.value(); }
|
||||
template<typename OtherDerType> inline bool operator<=(const AutoDiffScalar<OtherDerType>& b) const { return m_value <= b.value(); }
|
||||
template<typename OtherDerType> inline bool operator> (const AutoDiffScalar<OtherDerType>& b) const { return m_value > b.value(); }
|
||||
template<typename OtherDerType> inline bool operator>=(const AutoDiffScalar<OtherDerType>& b) const { return m_value >= b.value(); }
|
||||
template<typename OtherDerType> inline bool operator==(const AutoDiffScalar<OtherDerType>& b) const { return m_value == b.value(); }
|
||||
template<typename OtherDerType> inline bool operator!=(const AutoDiffScalar<OtherDerType>& b) const { return m_value != b.value(); }
|
||||
|
||||
inline const AutoDiffScalar<DerType&> operator+(const Scalar& other) const
|
||||
{
|
||||
return AutoDiffScalar<DerType&>(m_value + other, m_derivatives);
|
||||
}
|
||||
|
||||
friend inline const AutoDiffScalar<DerType&> operator+(const Scalar& a, const AutoDiffScalar& b)
|
||||
{
|
||||
return AutoDiffScalar<DerType&>(a + b.value(), b.derivatives());
|
||||
}
|
||||
|
||||
// inline const AutoDiffScalar<DerType&> operator+(const Real& other) const
|
||||
// {
|
||||
// return AutoDiffScalar<DerType&>(m_value + other, m_derivatives);
|
||||
// }
|
||||
|
||||
// friend inline const AutoDiffScalar<DerType&> operator+(const Real& a, const AutoDiffScalar& b)
|
||||
// {
|
||||
// return AutoDiffScalar<DerType&>(a + b.value(), b.derivatives());
|
||||
// }
|
||||
|
||||
inline AutoDiffScalar& operator+=(const Scalar& other)
|
||||
{
|
||||
value() += other;
|
||||
return *this;
|
||||
}
|
||||
|
||||
template<typename OtherDerType>
|
||||
inline const AutoDiffScalar<CwiseBinaryOp<internal::scalar_sum_op<Scalar>,const DerType,const typename internal::remove_all<OtherDerType>::type> >
|
||||
operator+(const AutoDiffScalar<OtherDerType>& other) const
|
||||
{
|
||||
internal::make_coherent(m_derivatives, other.derivatives());
|
||||
return AutoDiffScalar<CwiseBinaryOp<internal::scalar_sum_op<Scalar>,const DerType,const typename internal::remove_all<OtherDerType>::type> >(
|
||||
m_value + other.value(),
|
||||
m_derivatives + other.derivatives());
|
||||
}
|
||||
|
||||
template<typename OtherDerType>
|
||||
inline AutoDiffScalar&
|
||||
operator+=(const AutoDiffScalar<OtherDerType>& other)
|
||||
{
|
||||
(*this) = (*this) + other;
|
||||
return *this;
|
||||
}
|
||||
|
||||
inline const AutoDiffScalar<DerType&> operator-(const Scalar& b) const
|
||||
{
|
||||
return AutoDiffScalar<DerType&>(m_value - b, m_derivatives);
|
||||
}
|
||||
|
||||
friend inline const AutoDiffScalar<CwiseUnaryOp<internal::scalar_opposite_op<Scalar>, const DerType> >
|
||||
operator-(const Scalar& a, const AutoDiffScalar& b)
|
||||
{
|
||||
return AutoDiffScalar<CwiseUnaryOp<internal::scalar_opposite_op<Scalar>, const DerType> >
|
||||
(a - b.value(), -b.derivatives());
|
||||
}
|
||||
|
||||
inline AutoDiffScalar& operator-=(const Scalar& other)
|
||||
{
|
||||
value() -= other;
|
||||
return *this;
|
||||
}
|
||||
|
||||
template<typename OtherDerType>
|
||||
inline const AutoDiffScalar<CwiseBinaryOp<internal::scalar_difference_op<Scalar>, const DerType,const typename internal::remove_all<OtherDerType>::type> >
|
||||
operator-(const AutoDiffScalar<OtherDerType>& other) const
|
||||
{
|
||||
internal::make_coherent(m_derivatives, other.derivatives());
|
||||
return AutoDiffScalar<CwiseBinaryOp<internal::scalar_difference_op<Scalar>, const DerType,const typename internal::remove_all<OtherDerType>::type> >(
|
||||
m_value - other.value(),
|
||||
m_derivatives - other.derivatives());
|
||||
}
|
||||
|
||||
template<typename OtherDerType>
|
||||
inline AutoDiffScalar&
|
||||
operator-=(const AutoDiffScalar<OtherDerType>& other)
|
||||
{
|
||||
*this = *this - other;
|
||||
return *this;
|
||||
}
|
||||
|
||||
inline const AutoDiffScalar<CwiseUnaryOp<internal::scalar_opposite_op<Scalar>, const DerType> >
|
||||
operator-() const
|
||||
{
|
||||
return AutoDiffScalar<CwiseUnaryOp<internal::scalar_opposite_op<Scalar>, const DerType> >(
|
||||
-m_value,
|
||||
-m_derivatives);
|
||||
}
|
||||
|
||||
inline const AutoDiffScalar<EIGEN_EXPR_BINARYOP_SCALAR_RETURN_TYPE(DerType,Scalar,product) >
|
||||
operator*(const Scalar& other) const
|
||||
{
|
||||
return MakeAutoDiffScalar(m_value * other, m_derivatives * other);
|
||||
}
|
||||
|
||||
friend inline const AutoDiffScalar<EIGEN_EXPR_BINARYOP_SCALAR_RETURN_TYPE(DerType,Scalar,product) >
|
||||
operator*(const Scalar& other, const AutoDiffScalar& a)
|
||||
{
|
||||
return MakeAutoDiffScalar(a.value() * other, a.derivatives() * other);
|
||||
}
|
||||
|
||||
// inline const AutoDiffScalar<typename CwiseUnaryOp<internal::scalar_multiple_op<Real>, DerType>::Type >
|
||||
// operator*(const Real& other) const
|
||||
// {
|
||||
// return AutoDiffScalar<typename CwiseUnaryOp<internal::scalar_multiple_op<Real>, DerType>::Type >(
|
||||
// m_value * other,
|
||||
// (m_derivatives * other));
|
||||
// }
|
||||
//
|
||||
// friend inline const AutoDiffScalar<typename CwiseUnaryOp<internal::scalar_multiple_op<Real>, DerType>::Type >
|
||||
// operator*(const Real& other, const AutoDiffScalar& a)
|
||||
// {
|
||||
// return AutoDiffScalar<typename CwiseUnaryOp<internal::scalar_multiple_op<Real>, DerType>::Type >(
|
||||
// a.value() * other,
|
||||
// a.derivatives() * other);
|
||||
// }
|
||||
|
||||
inline const AutoDiffScalar<EIGEN_EXPR_BINARYOP_SCALAR_RETURN_TYPE(DerType,Scalar,product) >
|
||||
operator/(const Scalar& other) const
|
||||
{
|
||||
return MakeAutoDiffScalar(m_value / other, (m_derivatives * (Scalar(1)/other)));
|
||||
}
|
||||
|
||||
friend inline const AutoDiffScalar<EIGEN_EXPR_BINARYOP_SCALAR_RETURN_TYPE(DerType,Scalar,product) >
|
||||
operator/(const Scalar& other, const AutoDiffScalar& a)
|
||||
{
|
||||
return MakeAutoDiffScalar(other / a.value(), a.derivatives() * (Scalar(-other) / (a.value()*a.value())));
|
||||
}
|
||||
|
||||
// inline const AutoDiffScalar<typename CwiseUnaryOp<internal::scalar_multiple_op<Real>, DerType>::Type >
|
||||
// operator/(const Real& other) const
|
||||
// {
|
||||
// return AutoDiffScalar<typename CwiseUnaryOp<internal::scalar_multiple_op<Real>, DerType>::Type >(
|
||||
// m_value / other,
|
||||
// (m_derivatives * (Real(1)/other)));
|
||||
// }
|
||||
//
|
||||
// friend inline const AutoDiffScalar<typename CwiseUnaryOp<internal::scalar_multiple_op<Real>, DerType>::Type >
|
||||
// operator/(const Real& other, const AutoDiffScalar& a)
|
||||
// {
|
||||
// return AutoDiffScalar<typename CwiseUnaryOp<internal::scalar_multiple_op<Real>, DerType>::Type >(
|
||||
// other / a.value(),
|
||||
// a.derivatives() * (-Real(1)/other));
|
||||
// }
|
||||
|
||||
template<typename OtherDerType>
|
||||
inline const AutoDiffScalar<EIGEN_EXPR_BINARYOP_SCALAR_RETURN_TYPE(
|
||||
CwiseBinaryOp<internal::scalar_difference_op<Scalar> EIGEN_COMMA
|
||||
const EIGEN_EXPR_BINARYOP_SCALAR_RETURN_TYPE(DerType,Scalar,product) EIGEN_COMMA
|
||||
const EIGEN_EXPR_BINARYOP_SCALAR_RETURN_TYPE(typename internal::remove_all<OtherDerType>::type,Scalar,product) >,Scalar,product) >
|
||||
operator/(const AutoDiffScalar<OtherDerType>& other) const
|
||||
{
|
||||
internal::make_coherent(m_derivatives, other.derivatives());
|
||||
return MakeAutoDiffScalar(
|
||||
m_value / other.value(),
|
||||
((m_derivatives * other.value()) - (other.derivatives() * m_value))
|
||||
* (Scalar(1)/(other.value()*other.value())));
|
||||
}
|
||||
|
||||
template<typename OtherDerType>
|
||||
inline const AutoDiffScalar<CwiseBinaryOp<internal::scalar_sum_op<Scalar>,
|
||||
const EIGEN_EXPR_BINARYOP_SCALAR_RETURN_TYPE(DerType,Scalar,product),
|
||||
const EIGEN_EXPR_BINARYOP_SCALAR_RETURN_TYPE(typename internal::remove_all<OtherDerType>::type,Scalar,product) > >
|
||||
operator*(const AutoDiffScalar<OtherDerType>& other) const
|
||||
{
|
||||
internal::make_coherent(m_derivatives, other.derivatives());
|
||||
return MakeAutoDiffScalar(
|
||||
m_value * other.value(),
|
||||
(m_derivatives * other.value()) + (other.derivatives() * m_value));
|
||||
}
|
||||
|
||||
inline AutoDiffScalar& operator*=(const Scalar& other)
|
||||
{
|
||||
*this = *this * other;
|
||||
return *this;
|
||||
}
|
||||
|
||||
template<typename OtherDerType>
|
||||
inline AutoDiffScalar& operator*=(const AutoDiffScalar<OtherDerType>& other)
|
||||
{
|
||||
*this = *this * other;
|
||||
return *this;
|
||||
}
|
||||
|
||||
inline AutoDiffScalar& operator/=(const Scalar& other)
|
||||
{
|
||||
*this = *this / other;
|
||||
return *this;
|
||||
}
|
||||
|
||||
template<typename OtherDerType>
|
||||
inline AutoDiffScalar& operator/=(const AutoDiffScalar<OtherDerType>& other)
|
||||
{
|
||||
*this = *this / other;
|
||||
return *this;
|
||||
}
|
||||
|
||||
protected:
|
||||
Scalar m_value;
|
||||
DerType m_derivatives;
|
||||
|
||||
};
|
||||
|
||||
namespace internal {
|
||||
|
||||
template<typename _DerType>
|
||||
struct auto_diff_special_op<_DerType, true>
|
||||
// : auto_diff_scalar_op<_DerType, typename NumTraits<Scalar>::Real,
|
||||
// is_same<Scalar,typename NumTraits<Scalar>::Real>::value>
|
||||
{
|
||||
typedef typename remove_all<_DerType>::type DerType;
|
||||
typedef typename traits<DerType>::Scalar Scalar;
|
||||
typedef typename NumTraits<Scalar>::Real Real;
|
||||
|
||||
// typedef auto_diff_scalar_op<_DerType, typename NumTraits<Scalar>::Real,
|
||||
// is_same<Scalar,typename NumTraits<Scalar>::Real>::value> Base;
|
||||
|
||||
// using Base::operator+;
|
||||
// using Base::operator+=;
|
||||
// using Base::operator-;
|
||||
// using Base::operator-=;
|
||||
// using Base::operator*;
|
||||
// using Base::operator*=;
|
||||
|
||||
const AutoDiffScalar<_DerType>& derived() const { return *static_cast<const AutoDiffScalar<_DerType>*>(this); }
|
||||
AutoDiffScalar<_DerType>& derived() { return *static_cast<AutoDiffScalar<_DerType>*>(this); }
|
||||
|
||||
|
||||
inline const AutoDiffScalar<DerType&> operator+(const Real& other) const
|
||||
{
|
||||
return AutoDiffScalar<DerType&>(derived().value() + other, derived().derivatives());
|
||||
}
|
||||
|
||||
friend inline const AutoDiffScalar<DerType&> operator+(const Real& a, const AutoDiffScalar<_DerType>& b)
|
||||
{
|
||||
return AutoDiffScalar<DerType&>(a + b.value(), b.derivatives());
|
||||
}
|
||||
|
||||
inline AutoDiffScalar<_DerType>& operator+=(const Real& other)
|
||||
{
|
||||
derived().value() += other;
|
||||
return derived();
|
||||
}
|
||||
|
||||
|
||||
inline const AutoDiffScalar<typename CwiseUnaryOp<bind2nd_op<scalar_product_op<Scalar,Real> >, DerType>::Type >
|
||||
operator*(const Real& other) const
|
||||
{
|
||||
return AutoDiffScalar<typename CwiseUnaryOp<bind2nd_op<scalar_product_op<Scalar,Real> >, DerType>::Type >(
|
||||
derived().value() * other,
|
||||
derived().derivatives() * other);
|
||||
}
|
||||
|
||||
friend inline const AutoDiffScalar<typename CwiseUnaryOp<bind1st_op<scalar_product_op<Real,Scalar> >, DerType>::Type >
|
||||
operator*(const Real& other, const AutoDiffScalar<_DerType>& a)
|
||||
{
|
||||
return AutoDiffScalar<typename CwiseUnaryOp<bind1st_op<scalar_product_op<Real,Scalar> >, DerType>::Type >(
|
||||
a.value() * other,
|
||||
a.derivatives() * other);
|
||||
}
|
||||
|
||||
inline AutoDiffScalar<_DerType>& operator*=(const Scalar& other)
|
||||
{
|
||||
*this = *this * other;
|
||||
return derived();
|
||||
}
|
||||
};
|
||||
|
||||
template<typename _DerType>
|
||||
struct auto_diff_special_op<_DerType, false>
|
||||
{
|
||||
void operator*() const;
|
||||
void operator-() const;
|
||||
void operator+() const;
|
||||
};
|
||||
|
||||
template<typename BinOp, typename A, typename B, typename RefType>
|
||||
void make_coherent_expression(CwiseBinaryOp<BinOp,A,B> xpr, const RefType &ref)
|
||||
{
|
||||
make_coherent(xpr.const_cast_derived().lhs(), ref);
|
||||
make_coherent(xpr.const_cast_derived().rhs(), ref);
|
||||
}
|
||||
|
||||
template<typename UnaryOp, typename A, typename RefType>
|
||||
void make_coherent_expression(const CwiseUnaryOp<UnaryOp,A> &xpr, const RefType &ref)
|
||||
{
|
||||
make_coherent(xpr.nestedExpression().const_cast_derived(), ref);
|
||||
}
|
||||
|
||||
// needed for compilation only
|
||||
template<typename UnaryOp, typename A, typename RefType>
|
||||
void make_coherent_expression(const CwiseNullaryOp<UnaryOp,A> &, const RefType &)
|
||||
{}
|
||||
|
||||
template<typename A_Scalar, int A_Rows, int A_Cols, int A_Options, int A_MaxRows, int A_MaxCols, typename B>
|
||||
struct make_coherent_impl<Matrix<A_Scalar, A_Rows, A_Cols, A_Options, A_MaxRows, A_MaxCols>, B> {
|
||||
typedef Matrix<A_Scalar, A_Rows, A_Cols, A_Options, A_MaxRows, A_MaxCols> A;
|
||||
static void run(A& a, B& b) {
|
||||
if((A_Rows==Dynamic || A_Cols==Dynamic) && (a.size()==0))
|
||||
{
|
||||
a.resize(b.size());
|
||||
a.setZero();
|
||||
}
|
||||
else if (B::SizeAtCompileTime==Dynamic && a.size()!=0 && b.size()==0)
|
||||
{
|
||||
make_coherent_expression(b,a);
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
template<typename A, typename B_Scalar, int B_Rows, int B_Cols, int B_Options, int B_MaxRows, int B_MaxCols>
|
||||
struct make_coherent_impl<A, Matrix<B_Scalar, B_Rows, B_Cols, B_Options, B_MaxRows, B_MaxCols> > {
|
||||
typedef Matrix<B_Scalar, B_Rows, B_Cols, B_Options, B_MaxRows, B_MaxCols> B;
|
||||
static void run(A& a, B& b) {
|
||||
if((B_Rows==Dynamic || B_Cols==Dynamic) && (b.size()==0))
|
||||
{
|
||||
b.resize(a.size());
|
||||
b.setZero();
|
||||
}
|
||||
else if (A::SizeAtCompileTime==Dynamic && b.size()!=0 && a.size()==0)
|
||||
{
|
||||
make_coherent_expression(a,b);
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
template<typename A_Scalar, int A_Rows, int A_Cols, int A_Options, int A_MaxRows, int A_MaxCols,
|
||||
typename B_Scalar, int B_Rows, int B_Cols, int B_Options, int B_MaxRows, int B_MaxCols>
|
||||
struct make_coherent_impl<Matrix<A_Scalar, A_Rows, A_Cols, A_Options, A_MaxRows, A_MaxCols>,
|
||||
Matrix<B_Scalar, B_Rows, B_Cols, B_Options, B_MaxRows, B_MaxCols> > {
|
||||
typedef Matrix<A_Scalar, A_Rows, A_Cols, A_Options, A_MaxRows, A_MaxCols> A;
|
||||
typedef Matrix<B_Scalar, B_Rows, B_Cols, B_Options, B_MaxRows, B_MaxCols> B;
|
||||
static void run(A& a, B& b) {
|
||||
if((A_Rows==Dynamic || A_Cols==Dynamic) && (a.size()==0))
|
||||
{
|
||||
a.resize(b.size());
|
||||
a.setZero();
|
||||
}
|
||||
else if((B_Rows==Dynamic || B_Cols==Dynamic) && (b.size()==0))
|
||||
{
|
||||
b.resize(a.size());
|
||||
b.setZero();
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
} // end namespace internal
|
||||
|
||||
template<typename DerType, typename BinOp>
|
||||
struct ScalarBinaryOpTraits<AutoDiffScalar<DerType>,typename DerType::Scalar,BinOp>
|
||||
{
|
||||
typedef AutoDiffScalar<DerType> ReturnType;
|
||||
};
|
||||
|
||||
template<typename DerType, typename BinOp>
|
||||
struct ScalarBinaryOpTraits<typename DerType::Scalar,AutoDiffScalar<DerType>, BinOp>
|
||||
{
|
||||
typedef AutoDiffScalar<DerType> ReturnType;
|
||||
};
|
||||
|
||||
|
||||
// The following is an attempt to let Eigen's known about expression template, but that's more tricky!
|
||||
|
||||
// template<typename DerType, typename BinOp>
|
||||
// struct ScalarBinaryOpTraits<AutoDiffScalar<DerType>,AutoDiffScalar<DerType>, BinOp>
|
||||
// {
|
||||
// enum { Defined = 1 };
|
||||
// typedef AutoDiffScalar<typename DerType::PlainObject> ReturnType;
|
||||
// };
|
||||
//
|
||||
// template<typename DerType1,typename DerType2, typename BinOp>
|
||||
// struct ScalarBinaryOpTraits<AutoDiffScalar<DerType1>,AutoDiffScalar<DerType2>, BinOp>
|
||||
// {
|
||||
// enum { Defined = 1 };//internal::is_same<typename DerType1::Scalar,typename DerType2::Scalar>::value };
|
||||
// typedef AutoDiffScalar<typename DerType1::PlainObject> ReturnType;
|
||||
// };
|
||||
|
||||
#define EIGEN_AUTODIFF_DECLARE_GLOBAL_UNARY(FUNC,CODE) \
|
||||
template<typename DerType> \
|
||||
inline const Eigen::AutoDiffScalar< \
|
||||
EIGEN_EXPR_BINARYOP_SCALAR_RETURN_TYPE(typename Eigen::internal::remove_all<DerType>::type, typename Eigen::internal::traits<typename Eigen::internal::remove_all<DerType>::type>::Scalar, product) > \
|
||||
FUNC(const Eigen::AutoDiffScalar<DerType>& x) { \
|
||||
using namespace Eigen; \
|
||||
typedef typename Eigen::internal::traits<typename Eigen::internal::remove_all<DerType>::type>::Scalar Scalar; \
|
||||
EIGEN_UNUSED_VARIABLE(sizeof(Scalar)); \
|
||||
CODE; \
|
||||
}
|
||||
|
||||
template<typename DerType>
|
||||
inline const AutoDiffScalar<DerType>& conj(const AutoDiffScalar<DerType>& x) { return x; }
|
||||
template<typename DerType>
|
||||
inline const AutoDiffScalar<DerType>& real(const AutoDiffScalar<DerType>& x) { return x; }
|
||||
template<typename DerType>
|
||||
inline typename DerType::Scalar imag(const AutoDiffScalar<DerType>&) { return 0.; }
|
||||
template<typename DerType, typename T>
|
||||
inline AutoDiffScalar<typename Eigen::internal::remove_all<DerType>::type::PlainObject> (min)(const AutoDiffScalar<DerType>& x, const T& y) {
|
||||
typedef AutoDiffScalar<typename Eigen::internal::remove_all<DerType>::type::PlainObject> ADS;
|
||||
return (x <= y ? ADS(x) : ADS(y));
|
||||
}
|
||||
template<typename DerType, typename T>
|
||||
inline AutoDiffScalar<typename Eigen::internal::remove_all<DerType>::type::PlainObject> (max)(const AutoDiffScalar<DerType>& x, const T& y) {
|
||||
typedef AutoDiffScalar<typename Eigen::internal::remove_all<DerType>::type::PlainObject> ADS;
|
||||
return (x >= y ? ADS(x) : ADS(y));
|
||||
}
|
||||
template<typename DerType, typename T>
|
||||
inline AutoDiffScalar<typename Eigen::internal::remove_all<DerType>::type::PlainObject> (min)(const T& x, const AutoDiffScalar<DerType>& y) {
|
||||
typedef AutoDiffScalar<typename Eigen::internal::remove_all<DerType>::type::PlainObject> ADS;
|
||||
return (x < y ? ADS(x) : ADS(y));
|
||||
}
|
||||
template<typename DerType, typename T>
|
||||
inline AutoDiffScalar<typename Eigen::internal::remove_all<DerType>::type::PlainObject> (max)(const T& x, const AutoDiffScalar<DerType>& y) {
|
||||
typedef AutoDiffScalar<typename Eigen::internal::remove_all<DerType>::type::PlainObject> ADS;
|
||||
return (x > y ? ADS(x) : ADS(y));
|
||||
}
|
||||
template<typename DerType>
|
||||
inline AutoDiffScalar<typename Eigen::internal::remove_all<DerType>::type::PlainObject> (min)(const AutoDiffScalar<DerType>& x, const AutoDiffScalar<DerType>& y) {
|
||||
return (x.value() < y.value() ? x : y);
|
||||
}
|
||||
template<typename DerType>
|
||||
inline AutoDiffScalar<typename Eigen::internal::remove_all<DerType>::type::PlainObject> (max)(const AutoDiffScalar<DerType>& x, const AutoDiffScalar<DerType>& y) {
|
||||
return (x.value() >= y.value() ? x : y);
|
||||
}
|
||||
|
||||
|
||||
EIGEN_AUTODIFF_DECLARE_GLOBAL_UNARY(abs,
|
||||
using std::abs;
|
||||
return Eigen::MakeAutoDiffScalar(abs(x.value()), x.derivatives() * (x.value()<0 ? -1 : 1) );)
|
||||
|
||||
EIGEN_AUTODIFF_DECLARE_GLOBAL_UNARY(abs2,
|
||||
using numext::abs2;
|
||||
return Eigen::MakeAutoDiffScalar(abs2(x.value()), x.derivatives() * (Scalar(2)*x.value()));)
|
||||
|
||||
EIGEN_AUTODIFF_DECLARE_GLOBAL_UNARY(sqrt,
|
||||
using std::sqrt;
|
||||
Scalar sqrtx = sqrt(x.value());
|
||||
return Eigen::MakeAutoDiffScalar(sqrtx,x.derivatives() * (Scalar(0.5) / sqrtx));)
|
||||
|
||||
EIGEN_AUTODIFF_DECLARE_GLOBAL_UNARY(cos,
|
||||
using std::cos;
|
||||
using std::sin;
|
||||
return Eigen::MakeAutoDiffScalar(cos(x.value()), x.derivatives() * (-sin(x.value())));)
|
||||
|
||||
EIGEN_AUTODIFF_DECLARE_GLOBAL_UNARY(sin,
|
||||
using std::sin;
|
||||
using std::cos;
|
||||
return Eigen::MakeAutoDiffScalar(sin(x.value()),x.derivatives() * cos(x.value()));)
|
||||
|
||||
EIGEN_AUTODIFF_DECLARE_GLOBAL_UNARY(exp,
|
||||
using std::exp;
|
||||
Scalar expx = exp(x.value());
|
||||
return Eigen::MakeAutoDiffScalar(expx,x.derivatives() * expx);)
|
||||
|
||||
EIGEN_AUTODIFF_DECLARE_GLOBAL_UNARY(log,
|
||||
using std::log;
|
||||
return Eigen::MakeAutoDiffScalar(log(x.value()),x.derivatives() * (Scalar(1)/x.value()));)
|
||||
|
||||
template<typename DerType>
|
||||
inline const Eigen::AutoDiffScalar<
|
||||
EIGEN_EXPR_BINARYOP_SCALAR_RETURN_TYPE(typename internal::remove_all<DerType>::type,typename internal::traits<typename internal::remove_all<DerType>::type>::Scalar,product) >
|
||||
pow(const Eigen::AutoDiffScalar<DerType> &x, const typename internal::traits<typename internal::remove_all<DerType>::type>::Scalar &y)
|
||||
{
|
||||
using namespace Eigen;
|
||||
using std::pow;
|
||||
return Eigen::MakeAutoDiffScalar(pow(x.value(),y), x.derivatives() * (y * pow(x.value(),y-1)));
|
||||
}
|
||||
|
||||
|
||||
template<typename DerTypeA,typename DerTypeB>
|
||||
inline const AutoDiffScalar<Matrix<typename internal::traits<typename internal::remove_all<DerTypeA>::type>::Scalar,Dynamic,1> >
|
||||
atan2(const AutoDiffScalar<DerTypeA>& a, const AutoDiffScalar<DerTypeB>& b)
|
||||
{
|
||||
using std::atan2;
|
||||
typedef typename internal::traits<typename internal::remove_all<DerTypeA>::type>::Scalar Scalar;
|
||||
typedef AutoDiffScalar<Matrix<Scalar,Dynamic,1> > PlainADS;
|
||||
PlainADS ret;
|
||||
ret.value() = atan2(a.value(), b.value());
|
||||
|
||||
Scalar squared_hypot = a.value() * a.value() + b.value() * b.value();
|
||||
|
||||
// if (squared_hypot==0) the derivation is undefined and the following results in a NaN:
|
||||
ret.derivatives() = (a.derivatives() * b.value() - a.value() * b.derivatives()) / squared_hypot;
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
EIGEN_AUTODIFF_DECLARE_GLOBAL_UNARY(tan,
|
||||
using std::tan;
|
||||
using std::cos;
|
||||
return Eigen::MakeAutoDiffScalar(tan(x.value()),x.derivatives() * (Scalar(1)/numext::abs2(cos(x.value()))));)
|
||||
|
||||
EIGEN_AUTODIFF_DECLARE_GLOBAL_UNARY(asin,
|
||||
using std::sqrt;
|
||||
using std::asin;
|
||||
return Eigen::MakeAutoDiffScalar(asin(x.value()),x.derivatives() * (Scalar(1)/sqrt(1-numext::abs2(x.value()))));)
|
||||
|
||||
EIGEN_AUTODIFF_DECLARE_GLOBAL_UNARY(acos,
|
||||
using std::sqrt;
|
||||
using std::acos;
|
||||
return Eigen::MakeAutoDiffScalar(acos(x.value()),x.derivatives() * (Scalar(-1)/sqrt(1-numext::abs2(x.value()))));)
|
||||
|
||||
EIGEN_AUTODIFF_DECLARE_GLOBAL_UNARY(tanh,
|
||||
using std::cosh;
|
||||
using std::tanh;
|
||||
return Eigen::MakeAutoDiffScalar(tanh(x.value()),x.derivatives() * (Scalar(1)/numext::abs2(cosh(x.value()))));)
|
||||
|
||||
EIGEN_AUTODIFF_DECLARE_GLOBAL_UNARY(sinh,
|
||||
using std::sinh;
|
||||
using std::cosh;
|
||||
return Eigen::MakeAutoDiffScalar(sinh(x.value()),x.derivatives() * cosh(x.value()));)
|
||||
|
||||
EIGEN_AUTODIFF_DECLARE_GLOBAL_UNARY(cosh,
|
||||
using std::sinh;
|
||||
using std::cosh;
|
||||
return Eigen::MakeAutoDiffScalar(cosh(x.value()),x.derivatives() * sinh(x.value()));)
|
||||
|
||||
#undef EIGEN_AUTODIFF_DECLARE_GLOBAL_UNARY
|
||||
|
||||
template<typename DerType> struct NumTraits<AutoDiffScalar<DerType> >
|
||||
: NumTraits< typename NumTraits<typename internal::remove_all<DerType>::type::Scalar>::Real >
|
||||
{
|
||||
typedef typename internal::remove_all<DerType>::type DerTypeCleaned;
|
||||
typedef AutoDiffScalar<Matrix<typename NumTraits<typename DerTypeCleaned::Scalar>::Real,DerTypeCleaned::RowsAtCompileTime,DerTypeCleaned::ColsAtCompileTime,
|
||||
0, DerTypeCleaned::MaxRowsAtCompileTime, DerTypeCleaned::MaxColsAtCompileTime> > Real;
|
||||
typedef AutoDiffScalar<DerType> NonInteger;
|
||||
typedef AutoDiffScalar<DerType> Nested;
|
||||
typedef typename NumTraits<typename DerTypeCleaned::Scalar>::Literal Literal;
|
||||
enum{
|
||||
RequireInitialization = 1
|
||||
};
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
namespace std {
|
||||
template <typename T>
|
||||
class numeric_limits<Eigen::AutoDiffScalar<T> >
|
||||
: public numeric_limits<typename T::Scalar> {};
|
||||
|
||||
} // namespace std
|
||||
|
||||
#endif // EIGEN_AUTODIFF_SCALAR_H
|
||||
@@ -1,220 +0,0 @@
|
||||
// This file is part of Eigen, a lightweight C++ template library
|
||||
// for linear algebra.
|
||||
//
|
||||
// Copyright (C) 2009 Gael Guennebaud <gael.guennebaud@inria.fr>
|
||||
//
|
||||
// This Source Code Form is subject to the terms of the Mozilla
|
||||
// Public License v. 2.0. If a copy of the MPL was not distributed
|
||||
// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
|
||||
|
||||
#ifndef EIGEN_AUTODIFF_VECTOR_H
|
||||
#define EIGEN_AUTODIFF_VECTOR_H
|
||||
|
||||
namespace Eigen {
|
||||
|
||||
/* \class AutoDiffScalar
|
||||
* \brief A scalar type replacement with automatic differentation capability
|
||||
*
|
||||
* \param DerType the vector type used to store/represent the derivatives (e.g. Vector3f)
|
||||
*
|
||||
* This class represents a scalar value while tracking its respective derivatives.
|
||||
*
|
||||
* It supports the following list of global math function:
|
||||
* - std::abs, std::sqrt, std::pow, std::exp, std::log, std::sin, std::cos,
|
||||
* - internal::abs, internal::sqrt, numext::pow, internal::exp, internal::log, internal::sin, internal::cos,
|
||||
* - internal::conj, internal::real, internal::imag, numext::abs2.
|
||||
*
|
||||
* AutoDiffScalar can be used as the scalar type of an Eigen::Matrix object. However,
|
||||
* in that case, the expression template mechanism only occurs at the top Matrix level,
|
||||
* while derivatives are computed right away.
|
||||
*
|
||||
*/
|
||||
template<typename ValueType, typename JacobianType>
|
||||
class AutoDiffVector
|
||||
{
|
||||
public:
|
||||
//typedef typename internal::traits<ValueType>::Scalar Scalar;
|
||||
typedef typename internal::traits<ValueType>::Scalar BaseScalar;
|
||||
typedef AutoDiffScalar<Matrix<BaseScalar,JacobianType::RowsAtCompileTime,1> > ActiveScalar;
|
||||
typedef ActiveScalar Scalar;
|
||||
typedef AutoDiffScalar<typename JacobianType::ColXpr> CoeffType;
|
||||
typedef typename JacobianType::Index Index;
|
||||
|
||||
inline AutoDiffVector() {}
|
||||
|
||||
inline AutoDiffVector(const ValueType& values)
|
||||
: m_values(values)
|
||||
{
|
||||
m_jacobian.setZero();
|
||||
}
|
||||
|
||||
|
||||
CoeffType operator[] (Index i) { return CoeffType(m_values[i], m_jacobian.col(i)); }
|
||||
const CoeffType operator[] (Index i) const { return CoeffType(m_values[i], m_jacobian.col(i)); }
|
||||
|
||||
CoeffType operator() (Index i) { return CoeffType(m_values[i], m_jacobian.col(i)); }
|
||||
const CoeffType operator() (Index i) const { return CoeffType(m_values[i], m_jacobian.col(i)); }
|
||||
|
||||
CoeffType coeffRef(Index i) { return CoeffType(m_values[i], m_jacobian.col(i)); }
|
||||
const CoeffType coeffRef(Index i) const { return CoeffType(m_values[i], m_jacobian.col(i)); }
|
||||
|
||||
Index size() const { return m_values.size(); }
|
||||
|
||||
// FIXME here we could return an expression of the sum
|
||||
Scalar sum() const { /*std::cerr << "sum \n\n";*/ /*std::cerr << m_jacobian.rowwise().sum() << "\n\n";*/ return Scalar(m_values.sum(), m_jacobian.rowwise().sum()); }
|
||||
|
||||
|
||||
inline AutoDiffVector(const ValueType& values, const JacobianType& jac)
|
||||
: m_values(values), m_jacobian(jac)
|
||||
{}
|
||||
|
||||
template<typename OtherValueType, typename OtherJacobianType>
|
||||
inline AutoDiffVector(const AutoDiffVector<OtherValueType, OtherJacobianType>& other)
|
||||
: m_values(other.values()), m_jacobian(other.jacobian())
|
||||
{}
|
||||
|
||||
inline AutoDiffVector(const AutoDiffVector& other)
|
||||
: m_values(other.values()), m_jacobian(other.jacobian())
|
||||
{}
|
||||
|
||||
template<typename OtherValueType, typename OtherJacobianType>
|
||||
inline AutoDiffVector& operator=(const AutoDiffVector<OtherValueType, OtherJacobianType>& other)
|
||||
{
|
||||
m_values = other.values();
|
||||
m_jacobian = other.jacobian();
|
||||
return *this;
|
||||
}
|
||||
|
||||
inline AutoDiffVector& operator=(const AutoDiffVector& other)
|
||||
{
|
||||
m_values = other.values();
|
||||
m_jacobian = other.jacobian();
|
||||
return *this;
|
||||
}
|
||||
|
||||
inline const ValueType& values() const { return m_values; }
|
||||
inline ValueType& values() { return m_values; }
|
||||
|
||||
inline const JacobianType& jacobian() const { return m_jacobian; }
|
||||
inline JacobianType& jacobian() { return m_jacobian; }
|
||||
|
||||
template<typename OtherValueType,typename OtherJacobianType>
|
||||
inline const AutoDiffVector<
|
||||
typename MakeCwiseBinaryOp<internal::scalar_sum_op<BaseScalar>,ValueType,OtherValueType>::Type,
|
||||
typename MakeCwiseBinaryOp<internal::scalar_sum_op<BaseScalar>,JacobianType,OtherJacobianType>::Type >
|
||||
operator+(const AutoDiffVector<OtherValueType,OtherJacobianType>& other) const
|
||||
{
|
||||
return AutoDiffVector<
|
||||
typename MakeCwiseBinaryOp<internal::scalar_sum_op<BaseScalar>,ValueType,OtherValueType>::Type,
|
||||
typename MakeCwiseBinaryOp<internal::scalar_sum_op<BaseScalar>,JacobianType,OtherJacobianType>::Type >(
|
||||
m_values + other.values(),
|
||||
m_jacobian + other.jacobian());
|
||||
}
|
||||
|
||||
template<typename OtherValueType, typename OtherJacobianType>
|
||||
inline AutoDiffVector&
|
||||
operator+=(const AutoDiffVector<OtherValueType,OtherJacobianType>& other)
|
||||
{
|
||||
m_values += other.values();
|
||||
m_jacobian += other.jacobian();
|
||||
return *this;
|
||||
}
|
||||
|
||||
template<typename OtherValueType,typename OtherJacobianType>
|
||||
inline const AutoDiffVector<
|
||||
typename MakeCwiseBinaryOp<internal::scalar_difference_op<Scalar>,ValueType,OtherValueType>::Type,
|
||||
typename MakeCwiseBinaryOp<internal::scalar_difference_op<Scalar>,JacobianType,OtherJacobianType>::Type >
|
||||
operator-(const AutoDiffVector<OtherValueType,OtherJacobianType>& other) const
|
||||
{
|
||||
return AutoDiffVector<
|
||||
typename MakeCwiseBinaryOp<internal::scalar_difference_op<Scalar>,ValueType,OtherValueType>::Type,
|
||||
typename MakeCwiseBinaryOp<internal::scalar_difference_op<Scalar>,JacobianType,OtherJacobianType>::Type >(
|
||||
m_values - other.values(),
|
||||
m_jacobian - other.jacobian());
|
||||
}
|
||||
|
||||
template<typename OtherValueType, typename OtherJacobianType>
|
||||
inline AutoDiffVector&
|
||||
operator-=(const AutoDiffVector<OtherValueType,OtherJacobianType>& other)
|
||||
{
|
||||
m_values -= other.values();
|
||||
m_jacobian -= other.jacobian();
|
||||
return *this;
|
||||
}
|
||||
|
||||
inline const AutoDiffVector<
|
||||
typename MakeCwiseUnaryOp<internal::scalar_opposite_op<Scalar>, ValueType>::Type,
|
||||
typename MakeCwiseUnaryOp<internal::scalar_opposite_op<Scalar>, JacobianType>::Type >
|
||||
operator-() const
|
||||
{
|
||||
return AutoDiffVector<
|
||||
typename MakeCwiseUnaryOp<internal::scalar_opposite_op<Scalar>, ValueType>::Type,
|
||||
typename MakeCwiseUnaryOp<internal::scalar_opposite_op<Scalar>, JacobianType>::Type >(
|
||||
-m_values,
|
||||
-m_jacobian);
|
||||
}
|
||||
|
||||
inline const AutoDiffVector<
|
||||
typename MakeCwiseUnaryOp<internal::scalar_multiple_op<Scalar>, ValueType>::Type,
|
||||
typename MakeCwiseUnaryOp<internal::scalar_multiple_op<Scalar>, JacobianType>::Type>
|
||||
operator*(const BaseScalar& other) const
|
||||
{
|
||||
return AutoDiffVector<
|
||||
typename MakeCwiseUnaryOp<internal::scalar_multiple_op<Scalar>, ValueType>::Type,
|
||||
typename MakeCwiseUnaryOp<internal::scalar_multiple_op<Scalar>, JacobianType>::Type >(
|
||||
m_values * other,
|
||||
m_jacobian * other);
|
||||
}
|
||||
|
||||
friend inline const AutoDiffVector<
|
||||
typename MakeCwiseUnaryOp<internal::scalar_multiple_op<Scalar>, ValueType>::Type,
|
||||
typename MakeCwiseUnaryOp<internal::scalar_multiple_op<Scalar>, JacobianType>::Type >
|
||||
operator*(const Scalar& other, const AutoDiffVector& v)
|
||||
{
|
||||
return AutoDiffVector<
|
||||
typename MakeCwiseUnaryOp<internal::scalar_multiple_op<Scalar>, ValueType>::Type,
|
||||
typename MakeCwiseUnaryOp<internal::scalar_multiple_op<Scalar>, JacobianType>::Type >(
|
||||
v.values() * other,
|
||||
v.jacobian() * other);
|
||||
}
|
||||
|
||||
// template<typename OtherValueType,typename OtherJacobianType>
|
||||
// inline const AutoDiffVector<
|
||||
// CwiseBinaryOp<internal::scalar_multiple_op<Scalar>, ValueType, OtherValueType>
|
||||
// CwiseBinaryOp<internal::scalar_sum_op<Scalar>,
|
||||
// CwiseUnaryOp<internal::scalar_multiple_op<Scalar>, JacobianType>,
|
||||
// CwiseUnaryOp<internal::scalar_multiple_op<Scalar>, OtherJacobianType> > >
|
||||
// operator*(const AutoDiffVector<OtherValueType,OtherJacobianType>& other) const
|
||||
// {
|
||||
// return AutoDiffVector<
|
||||
// CwiseBinaryOp<internal::scalar_multiple_op<Scalar>, ValueType, OtherValueType>
|
||||
// CwiseBinaryOp<internal::scalar_sum_op<Scalar>,
|
||||
// CwiseUnaryOp<internal::scalar_multiple_op<Scalar>, JacobianType>,
|
||||
// CwiseUnaryOp<internal::scalar_multiple_op<Scalar>, OtherJacobianType> > >(
|
||||
// m_values.cwise() * other.values(),
|
||||
// (m_jacobian * other.values()) + (m_values * other.jacobian()));
|
||||
// }
|
||||
|
||||
inline AutoDiffVector& operator*=(const Scalar& other)
|
||||
{
|
||||
m_values *= other;
|
||||
m_jacobian *= other;
|
||||
return *this;
|
||||
}
|
||||
|
||||
template<typename OtherValueType,typename OtherJacobianType>
|
||||
inline AutoDiffVector& operator*=(const AutoDiffVector<OtherValueType,OtherJacobianType>& other)
|
||||
{
|
||||
*this = *this * other;
|
||||
return *this;
|
||||
}
|
||||
|
||||
protected:
|
||||
ValueType m_values;
|
||||
JacobianType m_jacobian;
|
||||
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
#endif // EIGEN_AUTODIFF_VECTOR_H
|
||||
@@ -1,442 +0,0 @@
|
||||
// This file is part of Eigen, a lightweight C++ template library
|
||||
// for linear algebra.
|
||||
//
|
||||
// Copyright (C) 2009, 2010, 2013 Jitse Niesen <jitse@maths.leeds.ac.uk>
|
||||
// Copyright (C) 2011, 2013 Chen-Pang He <jdh8@ms63.hinet.net>
|
||||
//
|
||||
// This Source Code Form is subject to the terms of the Mozilla
|
||||
// Public License v. 2.0. If a copy of the MPL was not distributed
|
||||
// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
|
||||
|
||||
#ifndef EIGEN_MATRIX_EXPONENTIAL
|
||||
#define EIGEN_MATRIX_EXPONENTIAL
|
||||
|
||||
#include "StemFunction.h"
|
||||
|
||||
namespace Eigen {
|
||||
namespace internal {
|
||||
|
||||
/** \brief Scaling operator.
|
||||
*
|
||||
* This struct is used by CwiseUnaryOp to scale a matrix by \f$ 2^{-s} \f$.
|
||||
*/
|
||||
template <typename RealScalar>
|
||||
struct MatrixExponentialScalingOp
|
||||
{
|
||||
/** \brief Constructor.
|
||||
*
|
||||
* \param[in] squarings The integer \f$ s \f$ in this document.
|
||||
*/
|
||||
MatrixExponentialScalingOp(int squarings) : m_squarings(squarings) { }
|
||||
|
||||
|
||||
/** \brief Scale a matrix coefficient.
|
||||
*
|
||||
* \param[in,out] x The scalar to be scaled, becoming \f$ 2^{-s} x \f$.
|
||||
*/
|
||||
inline const RealScalar operator() (const RealScalar& x) const
|
||||
{
|
||||
using std::ldexp;
|
||||
return ldexp(x, -m_squarings);
|
||||
}
|
||||
|
||||
typedef std::complex<RealScalar> ComplexScalar;
|
||||
|
||||
/** \brief Scale a matrix coefficient.
|
||||
*
|
||||
* \param[in,out] x The scalar to be scaled, becoming \f$ 2^{-s} x \f$.
|
||||
*/
|
||||
inline const ComplexScalar operator() (const ComplexScalar& x) const
|
||||
{
|
||||
using std::ldexp;
|
||||
return ComplexScalar(ldexp(x.real(), -m_squarings), ldexp(x.imag(), -m_squarings));
|
||||
}
|
||||
|
||||
private:
|
||||
int m_squarings;
|
||||
};
|
||||
|
||||
/** \brief Compute the (3,3)-Padé approximant to the exponential.
|
||||
*
|
||||
* After exit, \f$ (V+U)(V-U)^{-1} \f$ is the Padé
|
||||
* approximant of \f$ \exp(A) \f$ around \f$ A = 0 \f$.
|
||||
*/
|
||||
template <typename MatA, typename MatU, typename MatV>
|
||||
void matrix_exp_pade3(const MatA& A, MatU& U, MatV& V)
|
||||
{
|
||||
typedef typename MatA::PlainObject MatrixType;
|
||||
typedef typename NumTraits<typename traits<MatA>::Scalar>::Real RealScalar;
|
||||
const RealScalar b[] = {120.L, 60.L, 12.L, 1.L};
|
||||
const MatrixType A2 = A * A;
|
||||
const MatrixType tmp = b[3] * A2 + b[1] * MatrixType::Identity(A.rows(), A.cols());
|
||||
U.noalias() = A * tmp;
|
||||
V = b[2] * A2 + b[0] * MatrixType::Identity(A.rows(), A.cols());
|
||||
}
|
||||
|
||||
/** \brief Compute the (5,5)-Padé approximant to the exponential.
|
||||
*
|
||||
* After exit, \f$ (V+U)(V-U)^{-1} \f$ is the Padé
|
||||
* approximant of \f$ \exp(A) \f$ around \f$ A = 0 \f$.
|
||||
*/
|
||||
template <typename MatA, typename MatU, typename MatV>
|
||||
void matrix_exp_pade5(const MatA& A, MatU& U, MatV& V)
|
||||
{
|
||||
typedef typename MatA::PlainObject MatrixType;
|
||||
typedef typename NumTraits<typename traits<MatrixType>::Scalar>::Real RealScalar;
|
||||
const RealScalar b[] = {30240.L, 15120.L, 3360.L, 420.L, 30.L, 1.L};
|
||||
const MatrixType A2 = A * A;
|
||||
const MatrixType A4 = A2 * A2;
|
||||
const MatrixType tmp = b[5] * A4 + b[3] * A2 + b[1] * MatrixType::Identity(A.rows(), A.cols());
|
||||
U.noalias() = A * tmp;
|
||||
V = b[4] * A4 + b[2] * A2 + b[0] * MatrixType::Identity(A.rows(), A.cols());
|
||||
}
|
||||
|
||||
/** \brief Compute the (7,7)-Padé approximant to the exponential.
|
||||
*
|
||||
* After exit, \f$ (V+U)(V-U)^{-1} \f$ is the Padé
|
||||
* approximant of \f$ \exp(A) \f$ around \f$ A = 0 \f$.
|
||||
*/
|
||||
template <typename MatA, typename MatU, typename MatV>
|
||||
void matrix_exp_pade7(const MatA& A, MatU& U, MatV& V)
|
||||
{
|
||||
typedef typename MatA::PlainObject MatrixType;
|
||||
typedef typename NumTraits<typename traits<MatrixType>::Scalar>::Real RealScalar;
|
||||
const RealScalar b[] = {17297280.L, 8648640.L, 1995840.L, 277200.L, 25200.L, 1512.L, 56.L, 1.L};
|
||||
const MatrixType A2 = A * A;
|
||||
const MatrixType A4 = A2 * A2;
|
||||
const MatrixType A6 = A4 * A2;
|
||||
const MatrixType tmp = b[7] * A6 + b[5] * A4 + b[3] * A2
|
||||
+ b[1] * MatrixType::Identity(A.rows(), A.cols());
|
||||
U.noalias() = A * tmp;
|
||||
V = b[6] * A6 + b[4] * A4 + b[2] * A2 + b[0] * MatrixType::Identity(A.rows(), A.cols());
|
||||
|
||||
}
|
||||
|
||||
/** \brief Compute the (9,9)-Padé approximant to the exponential.
|
||||
*
|
||||
* After exit, \f$ (V+U)(V-U)^{-1} \f$ is the Padé
|
||||
* approximant of \f$ \exp(A) \f$ around \f$ A = 0 \f$.
|
||||
*/
|
||||
template <typename MatA, typename MatU, typename MatV>
|
||||
void matrix_exp_pade9(const MatA& A, MatU& U, MatV& V)
|
||||
{
|
||||
typedef typename MatA::PlainObject MatrixType;
|
||||
typedef typename NumTraits<typename traits<MatrixType>::Scalar>::Real RealScalar;
|
||||
const RealScalar b[] = {17643225600.L, 8821612800.L, 2075673600.L, 302702400.L, 30270240.L,
|
||||
2162160.L, 110880.L, 3960.L, 90.L, 1.L};
|
||||
const MatrixType A2 = A * A;
|
||||
const MatrixType A4 = A2 * A2;
|
||||
const MatrixType A6 = A4 * A2;
|
||||
const MatrixType A8 = A6 * A2;
|
||||
const MatrixType tmp = b[9] * A8 + b[7] * A6 + b[5] * A4 + b[3] * A2
|
||||
+ b[1] * MatrixType::Identity(A.rows(), A.cols());
|
||||
U.noalias() = A * tmp;
|
||||
V = b[8] * A8 + b[6] * A6 + b[4] * A4 + b[2] * A2 + b[0] * MatrixType::Identity(A.rows(), A.cols());
|
||||
}
|
||||
|
||||
/** \brief Compute the (13,13)-Padé approximant to the exponential.
|
||||
*
|
||||
* After exit, \f$ (V+U)(V-U)^{-1} \f$ is the Padé
|
||||
* approximant of \f$ \exp(A) \f$ around \f$ A = 0 \f$.
|
||||
*/
|
||||
template <typename MatA, typename MatU, typename MatV>
|
||||
void matrix_exp_pade13(const MatA& A, MatU& U, MatV& V)
|
||||
{
|
||||
typedef typename MatA::PlainObject MatrixType;
|
||||
typedef typename NumTraits<typename traits<MatrixType>::Scalar>::Real RealScalar;
|
||||
const RealScalar b[] = {64764752532480000.L, 32382376266240000.L, 7771770303897600.L,
|
||||
1187353796428800.L, 129060195264000.L, 10559470521600.L, 670442572800.L,
|
||||
33522128640.L, 1323241920.L, 40840800.L, 960960.L, 16380.L, 182.L, 1.L};
|
||||
const MatrixType A2 = A * A;
|
||||
const MatrixType A4 = A2 * A2;
|
||||
const MatrixType A6 = A4 * A2;
|
||||
V = b[13] * A6 + b[11] * A4 + b[9] * A2; // used for temporary storage
|
||||
MatrixType tmp = A6 * V;
|
||||
tmp += b[7] * A6 + b[5] * A4 + b[3] * A2 + b[1] * MatrixType::Identity(A.rows(), A.cols());
|
||||
U.noalias() = A * tmp;
|
||||
tmp = b[12] * A6 + b[10] * A4 + b[8] * A2;
|
||||
V.noalias() = A6 * tmp;
|
||||
V += b[6] * A6 + b[4] * A4 + b[2] * A2 + b[0] * MatrixType::Identity(A.rows(), A.cols());
|
||||
}
|
||||
|
||||
/** \brief Compute the (17,17)-Padé approximant to the exponential.
|
||||
*
|
||||
* After exit, \f$ (V+U)(V-U)^{-1} \f$ is the Padé
|
||||
* approximant of \f$ \exp(A) \f$ around \f$ A = 0 \f$.
|
||||
*
|
||||
* This function activates only if your long double is double-double or quadruple.
|
||||
*/
|
||||
#if LDBL_MANT_DIG > 64
|
||||
template <typename MatA, typename MatU, typename MatV>
|
||||
void matrix_exp_pade17(const MatA& A, MatU& U, MatV& V)
|
||||
{
|
||||
typedef typename MatA::PlainObject MatrixType;
|
||||
typedef typename NumTraits<typename traits<MatrixType>::Scalar>::Real RealScalar;
|
||||
const RealScalar b[] = {830034394580628357120000.L, 415017197290314178560000.L,
|
||||
100610229646136770560000.L, 15720348382208870400000.L,
|
||||
1774878043152614400000.L, 153822763739893248000.L, 10608466464820224000.L,
|
||||
595373117923584000.L, 27563570274240000.L, 1060137318240000.L,
|
||||
33924394183680.L, 899510451840.L, 19554575040.L, 341863200.L, 4651200.L,
|
||||
46512.L, 306.L, 1.L};
|
||||
const MatrixType A2 = A * A;
|
||||
const MatrixType A4 = A2 * A2;
|
||||
const MatrixType A6 = A4 * A2;
|
||||
const MatrixType A8 = A4 * A4;
|
||||
V = b[17] * A8 + b[15] * A6 + b[13] * A4 + b[11] * A2; // used for temporary storage
|
||||
MatrixType tmp = A8 * V;
|
||||
tmp += b[9] * A8 + b[7] * A6 + b[5] * A4 + b[3] * A2
|
||||
+ b[1] * MatrixType::Identity(A.rows(), A.cols());
|
||||
U.noalias() = A * tmp;
|
||||
tmp = b[16] * A8 + b[14] * A6 + b[12] * A4 + b[10] * A2;
|
||||
V.noalias() = tmp * A8;
|
||||
V += b[8] * A8 + b[6] * A6 + b[4] * A4 + b[2] * A2
|
||||
+ b[0] * MatrixType::Identity(A.rows(), A.cols());
|
||||
}
|
||||
#endif
|
||||
|
||||
template <typename MatrixType, typename RealScalar = typename NumTraits<typename traits<MatrixType>::Scalar>::Real>
|
||||
struct matrix_exp_computeUV
|
||||
{
|
||||
/** \brief Compute Padé approximant to the exponential.
|
||||
*
|
||||
* Computes \c U, \c V and \c squarings such that \f$ (V+U)(V-U)^{-1} \f$ is a Padé
|
||||
* approximant of \f$ \exp(2^{-\mbox{squarings}}M) \f$ around \f$ M = 0 \f$, where \f$ M \f$
|
||||
* denotes the matrix \c arg. The degree of the Padé approximant and the value of squarings
|
||||
* are chosen such that the approximation error is no more than the round-off error.
|
||||
*/
|
||||
static void run(const MatrixType& arg, MatrixType& U, MatrixType& V, int& squarings);
|
||||
};
|
||||
|
||||
template <typename MatrixType>
|
||||
struct matrix_exp_computeUV<MatrixType, float>
|
||||
{
|
||||
template <typename ArgType>
|
||||
static void run(const ArgType& arg, MatrixType& U, MatrixType& V, int& squarings)
|
||||
{
|
||||
using std::frexp;
|
||||
using std::pow;
|
||||
const float l1norm = arg.cwiseAbs().colwise().sum().maxCoeff();
|
||||
squarings = 0;
|
||||
if (l1norm < 4.258730016922831e-001f) {
|
||||
matrix_exp_pade3(arg, U, V);
|
||||
} else if (l1norm < 1.880152677804762e+000f) {
|
||||
matrix_exp_pade5(arg, U, V);
|
||||
} else {
|
||||
const float maxnorm = 3.925724783138660f;
|
||||
frexp(l1norm / maxnorm, &squarings);
|
||||
if (squarings < 0) squarings = 0;
|
||||
MatrixType A = arg.unaryExpr(MatrixExponentialScalingOp<float>(squarings));
|
||||
matrix_exp_pade7(A, U, V);
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
template <typename MatrixType>
|
||||
struct matrix_exp_computeUV<MatrixType, double>
|
||||
{
|
||||
typedef typename NumTraits<typename traits<MatrixType>::Scalar>::Real RealScalar;
|
||||
template <typename ArgType>
|
||||
static void run(const ArgType& arg, MatrixType& U, MatrixType& V, int& squarings)
|
||||
{
|
||||
using std::frexp;
|
||||
using std::pow;
|
||||
const RealScalar l1norm = arg.cwiseAbs().colwise().sum().maxCoeff();
|
||||
squarings = 0;
|
||||
if (l1norm < 1.495585217958292e-002) {
|
||||
matrix_exp_pade3(arg, U, V);
|
||||
} else if (l1norm < 2.539398330063230e-001) {
|
||||
matrix_exp_pade5(arg, U, V);
|
||||
} else if (l1norm < 9.504178996162932e-001) {
|
||||
matrix_exp_pade7(arg, U, V);
|
||||
} else if (l1norm < 2.097847961257068e+000) {
|
||||
matrix_exp_pade9(arg, U, V);
|
||||
} else {
|
||||
const RealScalar maxnorm = 5.371920351148152;
|
||||
frexp(l1norm / maxnorm, &squarings);
|
||||
if (squarings < 0) squarings = 0;
|
||||
MatrixType A = arg.unaryExpr(MatrixExponentialScalingOp<RealScalar>(squarings));
|
||||
matrix_exp_pade13(A, U, V);
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
template <typename MatrixType>
|
||||
struct matrix_exp_computeUV<MatrixType, long double>
|
||||
{
|
||||
template <typename ArgType>
|
||||
static void run(const ArgType& arg, MatrixType& U, MatrixType& V, int& squarings)
|
||||
{
|
||||
#if LDBL_MANT_DIG == 53 // double precision
|
||||
matrix_exp_computeUV<MatrixType, double>::run(arg, U, V, squarings);
|
||||
|
||||
#else
|
||||
|
||||
using std::frexp;
|
||||
using std::pow;
|
||||
const long double l1norm = arg.cwiseAbs().colwise().sum().maxCoeff();
|
||||
squarings = 0;
|
||||
|
||||
#if LDBL_MANT_DIG <= 64 // extended precision
|
||||
|
||||
if (l1norm < 4.1968497232266989671e-003L) {
|
||||
matrix_exp_pade3(arg, U, V);
|
||||
} else if (l1norm < 1.1848116734693823091e-001L) {
|
||||
matrix_exp_pade5(arg, U, V);
|
||||
} else if (l1norm < 5.5170388480686700274e-001L) {
|
||||
matrix_exp_pade7(arg, U, V);
|
||||
} else if (l1norm < 1.3759868875587845383e+000L) {
|
||||
matrix_exp_pade9(arg, U, V);
|
||||
} else {
|
||||
const long double maxnorm = 4.0246098906697353063L;
|
||||
frexp(l1norm / maxnorm, &squarings);
|
||||
if (squarings < 0) squarings = 0;
|
||||
MatrixType A = arg.unaryExpr(MatrixExponentialScalingOp<long double>(squarings));
|
||||
matrix_exp_pade13(A, U, V);
|
||||
}
|
||||
|
||||
#elif LDBL_MANT_DIG <= 106 // double-double
|
||||
|
||||
if (l1norm < 3.2787892205607026992947488108213e-005L) {
|
||||
matrix_exp_pade3(arg, U, V);
|
||||
} else if (l1norm < 6.4467025060072760084130906076332e-003L) {
|
||||
matrix_exp_pade5(arg, U, V);
|
||||
} else if (l1norm < 6.8988028496595374751374122881143e-002L) {
|
||||
matrix_exp_pade7(arg, U, V);
|
||||
} else if (l1norm < 2.7339737518502231741495857201670e-001L) {
|
||||
matrix_exp_pade9(arg, U, V);
|
||||
} else if (l1norm < 1.3203382096514474905666448850278e+000L) {
|
||||
matrix_exp_pade13(arg, U, V);
|
||||
} else {
|
||||
const long double maxnorm = 3.2579440895405400856599663723517L;
|
||||
frexp(l1norm / maxnorm, &squarings);
|
||||
if (squarings < 0) squarings = 0;
|
||||
MatrixType A = arg.unaryExpr(MatrixExponentialScalingOp<long double>(squarings));
|
||||
matrix_exp_pade17(A, U, V);
|
||||
}
|
||||
|
||||
#elif LDBL_MANT_DIG <= 112 // quadruple precison
|
||||
|
||||
if (l1norm < 1.639394610288918690547467954466970e-005L) {
|
||||
matrix_exp_pade3(arg, U, V);
|
||||
} else if (l1norm < 4.253237712165275566025884344433009e-003L) {
|
||||
matrix_exp_pade5(arg, U, V);
|
||||
} else if (l1norm < 5.125804063165764409885122032933142e-002L) {
|
||||
matrix_exp_pade7(arg, U, V);
|
||||
} else if (l1norm < 2.170000765161155195453205651889853e-001L) {
|
||||
matrix_exp_pade9(arg, U, V);
|
||||
} else if (l1norm < 1.125358383453143065081397882891878e+000L) {
|
||||
matrix_exp_pade13(arg, U, V);
|
||||
} else {
|
||||
const long double maxnorm = 2.884233277829519311757165057717815L;
|
||||
frexp(l1norm / maxnorm, &squarings);
|
||||
if (squarings < 0) squarings = 0;
|
||||
MatrixType A = arg.unaryExpr(MatrixExponentialScalingOp<long double>(squarings));
|
||||
matrix_exp_pade17(A, U, V);
|
||||
}
|
||||
|
||||
#else
|
||||
|
||||
// this case should be handled in compute()
|
||||
eigen_assert(false && "Bug in MatrixExponential");
|
||||
|
||||
#endif
|
||||
#endif // LDBL_MANT_DIG
|
||||
}
|
||||
};
|
||||
|
||||
template<typename T> struct is_exp_known_type : false_type {};
|
||||
template<> struct is_exp_known_type<float> : true_type {};
|
||||
template<> struct is_exp_known_type<double> : true_type {};
|
||||
#if LDBL_MANT_DIG <= 112
|
||||
template<> struct is_exp_known_type<long double> : true_type {};
|
||||
#endif
|
||||
|
||||
template <typename ArgType, typename ResultType>
|
||||
void matrix_exp_compute(const ArgType& arg, ResultType &result, true_type) // natively supported scalar type
|
||||
{
|
||||
typedef typename ArgType::PlainObject MatrixType;
|
||||
MatrixType U, V;
|
||||
int squarings;
|
||||
matrix_exp_computeUV<MatrixType>::run(arg, U, V, squarings); // Pade approximant is (U+V) / (-U+V)
|
||||
MatrixType numer = U + V;
|
||||
MatrixType denom = -U + V;
|
||||
result = denom.partialPivLu().solve(numer);
|
||||
for (int i=0; i<squarings; i++)
|
||||
result *= result; // undo scaling by repeated squaring
|
||||
}
|
||||
|
||||
|
||||
/* Computes the matrix exponential
|
||||
*
|
||||
* \param arg argument of matrix exponential (should be plain object)
|
||||
* \param result variable in which result will be stored
|
||||
*/
|
||||
template <typename ArgType, typename ResultType>
|
||||
void matrix_exp_compute(const ArgType& arg, ResultType &result, false_type) // default
|
||||
{
|
||||
typedef typename ArgType::PlainObject MatrixType;
|
||||
typedef typename traits<MatrixType>::Scalar Scalar;
|
||||
typedef typename NumTraits<Scalar>::Real RealScalar;
|
||||
typedef typename std::complex<RealScalar> ComplexScalar;
|
||||
result = arg.matrixFunction(internal::stem_function_exp<ComplexScalar>);
|
||||
}
|
||||
|
||||
} // end namespace Eigen::internal
|
||||
|
||||
/** \ingroup MatrixFunctions_Module
|
||||
*
|
||||
* \brief Proxy for the matrix exponential of some matrix (expression).
|
||||
*
|
||||
* \tparam Derived Type of the argument to the matrix exponential.
|
||||
*
|
||||
* This class holds the argument to the matrix exponential until it is assigned or evaluated for
|
||||
* some other reason (so the argument should not be changed in the meantime). It is the return type
|
||||
* of MatrixBase::exp() and most of the time this is the only way it is used.
|
||||
*/
|
||||
template<typename Derived> struct MatrixExponentialReturnValue
|
||||
: public ReturnByValue<MatrixExponentialReturnValue<Derived> >
|
||||
{
|
||||
typedef typename Derived::Index Index;
|
||||
public:
|
||||
/** \brief Constructor.
|
||||
*
|
||||
* \param src %Matrix (expression) forming the argument of the matrix exponential.
|
||||
*/
|
||||
MatrixExponentialReturnValue(const Derived& src) : m_src(src) { }
|
||||
|
||||
/** \brief Compute the matrix exponential.
|
||||
*
|
||||
* \param result the matrix exponential of \p src in the constructor.
|
||||
*/
|
||||
template <typename ResultType>
|
||||
inline void evalTo(ResultType& result) const
|
||||
{
|
||||
const typename internal::nested_eval<Derived, 10>::type tmp(m_src);
|
||||
internal::matrix_exp_compute(tmp, result, internal::is_exp_known_type<typename Derived::RealScalar>());
|
||||
}
|
||||
|
||||
Index rows() const { return m_src.rows(); }
|
||||
Index cols() const { return m_src.cols(); }
|
||||
|
||||
protected:
|
||||
const typename internal::ref_selector<Derived>::type m_src;
|
||||
};
|
||||
|
||||
namespace internal {
|
||||
template<typename Derived>
|
||||
struct traits<MatrixExponentialReturnValue<Derived> >
|
||||
{
|
||||
typedef typename Derived::PlainObject ReturnType;
|
||||
};
|
||||
}
|
||||
|
||||
template <typename Derived>
|
||||
const MatrixExponentialReturnValue<Derived> MatrixBase<Derived>::exp() const
|
||||
{
|
||||
eigen_assert(rows() == cols());
|
||||
return MatrixExponentialReturnValue<Derived>(derived());
|
||||
}
|
||||
|
||||
} // end namespace Eigen
|
||||
|
||||
#endif // EIGEN_MATRIX_EXPONENTIAL
|
||||
@@ -1,580 +0,0 @@
|
||||
// This file is part of Eigen, a lightweight C++ template library
|
||||
// for linear algebra.
|
||||
//
|
||||
// Copyright (C) 2009-2011, 2013 Jitse Niesen <jitse@maths.leeds.ac.uk>
|
||||
//
|
||||
// This Source Code Form is subject to the terms of the Mozilla
|
||||
// Public License v. 2.0. If a copy of the MPL was not distributed
|
||||
// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
|
||||
|
||||
#ifndef EIGEN_MATRIX_FUNCTION_H
|
||||
#define EIGEN_MATRIX_FUNCTION_H
|
||||
|
||||
#include "StemFunction.h"
|
||||
|
||||
|
||||
namespace Eigen {
|
||||
|
||||
namespace internal {
|
||||
|
||||
/** \brief Maximum distance allowed between eigenvalues to be considered "close". */
|
||||
static const float matrix_function_separation = 0.1f;
|
||||
|
||||
/** \ingroup MatrixFunctions_Module
|
||||
* \class MatrixFunctionAtomic
|
||||
* \brief Helper class for computing matrix functions of atomic matrices.
|
||||
*
|
||||
* Here, an atomic matrix is a triangular matrix whose diagonal entries are close to each other.
|
||||
*/
|
||||
template <typename MatrixType>
|
||||
class MatrixFunctionAtomic
|
||||
{
|
||||
public:
|
||||
|
||||
typedef typename MatrixType::Scalar Scalar;
|
||||
typedef typename stem_function<Scalar>::type StemFunction;
|
||||
|
||||
/** \brief Constructor
|
||||
* \param[in] f matrix function to compute.
|
||||
*/
|
||||
MatrixFunctionAtomic(StemFunction f) : m_f(f) { }
|
||||
|
||||
/** \brief Compute matrix function of atomic matrix
|
||||
* \param[in] A argument of matrix function, should be upper triangular and atomic
|
||||
* \returns f(A), the matrix function evaluated at the given matrix
|
||||
*/
|
||||
MatrixType compute(const MatrixType& A);
|
||||
|
||||
private:
|
||||
StemFunction* m_f;
|
||||
};
|
||||
|
||||
template <typename MatrixType>
|
||||
typename NumTraits<typename MatrixType::Scalar>::Real matrix_function_compute_mu(const MatrixType& A)
|
||||
{
|
||||
typedef typename plain_col_type<MatrixType>::type VectorType;
|
||||
typename MatrixType::Index rows = A.rows();
|
||||
const MatrixType N = MatrixType::Identity(rows, rows) - A;
|
||||
VectorType e = VectorType::Ones(rows);
|
||||
N.template triangularView<Upper>().solveInPlace(e);
|
||||
return e.cwiseAbs().maxCoeff();
|
||||
}
|
||||
|
||||
template <typename MatrixType>
|
||||
MatrixType MatrixFunctionAtomic<MatrixType>::compute(const MatrixType& A)
|
||||
{
|
||||
// TODO: Use that A is upper triangular
|
||||
typedef typename NumTraits<Scalar>::Real RealScalar;
|
||||
typedef typename MatrixType::Index Index;
|
||||
Index rows = A.rows();
|
||||
Scalar avgEival = A.trace() / Scalar(RealScalar(rows));
|
||||
MatrixType Ashifted = A - avgEival * MatrixType::Identity(rows, rows);
|
||||
RealScalar mu = matrix_function_compute_mu(Ashifted);
|
||||
MatrixType F = m_f(avgEival, 0) * MatrixType::Identity(rows, rows);
|
||||
MatrixType P = Ashifted;
|
||||
MatrixType Fincr;
|
||||
for (Index s = 1; s < 1.1 * rows + 10; s++) { // upper limit is fairly arbitrary
|
||||
Fincr = m_f(avgEival, static_cast<int>(s)) * P;
|
||||
F += Fincr;
|
||||
P = Scalar(RealScalar(1.0/(s + 1))) * P * Ashifted;
|
||||
|
||||
// test whether Taylor series converged
|
||||
const RealScalar F_norm = F.cwiseAbs().rowwise().sum().maxCoeff();
|
||||
const RealScalar Fincr_norm = Fincr.cwiseAbs().rowwise().sum().maxCoeff();
|
||||
if (Fincr_norm < NumTraits<Scalar>::epsilon() * F_norm) {
|
||||
RealScalar delta = 0;
|
||||
RealScalar rfactorial = 1;
|
||||
for (Index r = 0; r < rows; r++) {
|
||||
RealScalar mx = 0;
|
||||
for (Index i = 0; i < rows; i++)
|
||||
mx = (std::max)(mx, std::abs(m_f(Ashifted(i, i) + avgEival, static_cast<int>(s+r))));
|
||||
if (r != 0)
|
||||
rfactorial *= RealScalar(r);
|
||||
delta = (std::max)(delta, mx / rfactorial);
|
||||
}
|
||||
const RealScalar P_norm = P.cwiseAbs().rowwise().sum().maxCoeff();
|
||||
if (mu * delta * P_norm < NumTraits<Scalar>::epsilon() * F_norm) // series converged
|
||||
break;
|
||||
}
|
||||
}
|
||||
return F;
|
||||
}
|
||||
|
||||
/** \brief Find cluster in \p clusters containing some value
|
||||
* \param[in] key Value to find
|
||||
* \returns Iterator to cluster containing \p key, or \c clusters.end() if no cluster in \p m_clusters
|
||||
* contains \p key.
|
||||
*/
|
||||
template <typename Index, typename ListOfClusters>
|
||||
typename ListOfClusters::iterator matrix_function_find_cluster(Index key, ListOfClusters& clusters)
|
||||
{
|
||||
typename std::list<Index>::iterator j;
|
||||
for (typename ListOfClusters::iterator i = clusters.begin(); i != clusters.end(); ++i) {
|
||||
j = std::find(i->begin(), i->end(), key);
|
||||
if (j != i->end())
|
||||
return i;
|
||||
}
|
||||
return clusters.end();
|
||||
}
|
||||
|
||||
/** \brief Partition eigenvalues in clusters of ei'vals close to each other
|
||||
*
|
||||
* \param[in] eivals Eigenvalues
|
||||
* \param[out] clusters Resulting partition of eigenvalues
|
||||
*
|
||||
* The partition satisfies the following two properties:
|
||||
* # Any eigenvalue in a certain cluster is at most matrix_function_separation() away from another eigenvalue
|
||||
* in the same cluster.
|
||||
* # The distance between two eigenvalues in different clusters is more than matrix_function_separation().
|
||||
* The implementation follows Algorithm 4.1 in the paper of Davies and Higham.
|
||||
*/
|
||||
template <typename EivalsType, typename Cluster>
|
||||
void matrix_function_partition_eigenvalues(const EivalsType& eivals, std::list<Cluster>& clusters)
|
||||
{
|
||||
typedef typename EivalsType::Index Index;
|
||||
typedef typename EivalsType::RealScalar RealScalar;
|
||||
for (Index i=0; i<eivals.rows(); ++i) {
|
||||
// Find cluster containing i-th ei'val, adding a new cluster if necessary
|
||||
typename std::list<Cluster>::iterator qi = matrix_function_find_cluster(i, clusters);
|
||||
if (qi == clusters.end()) {
|
||||
Cluster l;
|
||||
l.push_back(i);
|
||||
clusters.push_back(l);
|
||||
qi = clusters.end();
|
||||
--qi;
|
||||
}
|
||||
|
||||
// Look for other element to add to the set
|
||||
for (Index j=i+1; j<eivals.rows(); ++j) {
|
||||
if (abs(eivals(j) - eivals(i)) <= RealScalar(matrix_function_separation)
|
||||
&& std::find(qi->begin(), qi->end(), j) == qi->end()) {
|
||||
typename std::list<Cluster>::iterator qj = matrix_function_find_cluster(j, clusters);
|
||||
if (qj == clusters.end()) {
|
||||
qi->push_back(j);
|
||||
} else {
|
||||
qi->insert(qi->end(), qj->begin(), qj->end());
|
||||
clusters.erase(qj);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/** \brief Compute size of each cluster given a partitioning */
|
||||
template <typename ListOfClusters, typename Index>
|
||||
void matrix_function_compute_cluster_size(const ListOfClusters& clusters, Matrix<Index, Dynamic, 1>& clusterSize)
|
||||
{
|
||||
const Index numClusters = static_cast<Index>(clusters.size());
|
||||
clusterSize.setZero(numClusters);
|
||||
Index clusterIndex = 0;
|
||||
for (typename ListOfClusters::const_iterator cluster = clusters.begin(); cluster != clusters.end(); ++cluster) {
|
||||
clusterSize[clusterIndex] = cluster->size();
|
||||
++clusterIndex;
|
||||
}
|
||||
}
|
||||
|
||||
/** \brief Compute start of each block using clusterSize */
|
||||
template <typename VectorType>
|
||||
void matrix_function_compute_block_start(const VectorType& clusterSize, VectorType& blockStart)
|
||||
{
|
||||
blockStart.resize(clusterSize.rows());
|
||||
blockStart(0) = 0;
|
||||
for (typename VectorType::Index i = 1; i < clusterSize.rows(); i++) {
|
||||
blockStart(i) = blockStart(i-1) + clusterSize(i-1);
|
||||
}
|
||||
}
|
||||
|
||||
/** \brief Compute mapping of eigenvalue indices to cluster indices */
|
||||
template <typename EivalsType, typename ListOfClusters, typename VectorType>
|
||||
void matrix_function_compute_map(const EivalsType& eivals, const ListOfClusters& clusters, VectorType& eivalToCluster)
|
||||
{
|
||||
typedef typename EivalsType::Index Index;
|
||||
eivalToCluster.resize(eivals.rows());
|
||||
Index clusterIndex = 0;
|
||||
for (typename ListOfClusters::const_iterator cluster = clusters.begin(); cluster != clusters.end(); ++cluster) {
|
||||
for (Index i = 0; i < eivals.rows(); ++i) {
|
||||
if (std::find(cluster->begin(), cluster->end(), i) != cluster->end()) {
|
||||
eivalToCluster[i] = clusterIndex;
|
||||
}
|
||||
}
|
||||
++clusterIndex;
|
||||
}
|
||||
}
|
||||
|
||||
/** \brief Compute permutation which groups ei'vals in same cluster together */
|
||||
template <typename DynVectorType, typename VectorType>
|
||||
void matrix_function_compute_permutation(const DynVectorType& blockStart, const DynVectorType& eivalToCluster, VectorType& permutation)
|
||||
{
|
||||
typedef typename VectorType::Index Index;
|
||||
DynVectorType indexNextEntry = blockStart;
|
||||
permutation.resize(eivalToCluster.rows());
|
||||
for (Index i = 0; i < eivalToCluster.rows(); i++) {
|
||||
Index cluster = eivalToCluster[i];
|
||||
permutation[i] = indexNextEntry[cluster];
|
||||
++indexNextEntry[cluster];
|
||||
}
|
||||
}
|
||||
|
||||
/** \brief Permute Schur decomposition in U and T according to permutation */
|
||||
template <typename VectorType, typename MatrixType>
|
||||
void matrix_function_permute_schur(VectorType& permutation, MatrixType& U, MatrixType& T)
|
||||
{
|
||||
typedef typename VectorType::Index Index;
|
||||
for (Index i = 0; i < permutation.rows() - 1; i++) {
|
||||
Index j;
|
||||
for (j = i; j < permutation.rows(); j++) {
|
||||
if (permutation(j) == i) break;
|
||||
}
|
||||
eigen_assert(permutation(j) == i);
|
||||
for (Index k = j-1; k >= i; k--) {
|
||||
JacobiRotation<typename MatrixType::Scalar> rotation;
|
||||
rotation.makeGivens(T(k, k+1), T(k+1, k+1) - T(k, k));
|
||||
T.applyOnTheLeft(k, k+1, rotation.adjoint());
|
||||
T.applyOnTheRight(k, k+1, rotation);
|
||||
U.applyOnTheRight(k, k+1, rotation);
|
||||
std::swap(permutation.coeffRef(k), permutation.coeffRef(k+1));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/** \brief Compute block diagonal part of matrix function.
|
||||
*
|
||||
* This routine computes the matrix function applied to the block diagonal part of \p T (which should be
|
||||
* upper triangular), with the blocking given by \p blockStart and \p clusterSize. The matrix function of
|
||||
* each diagonal block is computed by \p atomic. The off-diagonal parts of \p fT are set to zero.
|
||||
*/
|
||||
template <typename MatrixType, typename AtomicType, typename VectorType>
|
||||
void matrix_function_compute_block_atomic(const MatrixType& T, AtomicType& atomic, const VectorType& blockStart, const VectorType& clusterSize, MatrixType& fT)
|
||||
{
|
||||
fT.setZero(T.rows(), T.cols());
|
||||
for (typename VectorType::Index i = 0; i < clusterSize.rows(); ++i) {
|
||||
fT.block(blockStart(i), blockStart(i), clusterSize(i), clusterSize(i))
|
||||
= atomic.compute(T.block(blockStart(i), blockStart(i), clusterSize(i), clusterSize(i)));
|
||||
}
|
||||
}
|
||||
|
||||
/** \brief Solve a triangular Sylvester equation AX + XB = C
|
||||
*
|
||||
* \param[in] A the matrix A; should be square and upper triangular
|
||||
* \param[in] B the matrix B; should be square and upper triangular
|
||||
* \param[in] C the matrix C; should have correct size.
|
||||
*
|
||||
* \returns the solution X.
|
||||
*
|
||||
* If A is m-by-m and B is n-by-n, then both C and X are m-by-n. The (i,j)-th component of the Sylvester
|
||||
* equation is
|
||||
* \f[
|
||||
* \sum_{k=i}^m A_{ik} X_{kj} + \sum_{k=1}^j X_{ik} B_{kj} = C_{ij}.
|
||||
* \f]
|
||||
* This can be re-arranged to yield:
|
||||
* \f[
|
||||
* X_{ij} = \frac{1}{A_{ii} + B_{jj}} \Bigl( C_{ij}
|
||||
* - \sum_{k=i+1}^m A_{ik} X_{kj} - \sum_{k=1}^{j-1} X_{ik} B_{kj} \Bigr).
|
||||
* \f]
|
||||
* It is assumed that A and B are such that the numerator is never zero (otherwise the Sylvester equation
|
||||
* does not have a unique solution). In that case, these equations can be evaluated in the order
|
||||
* \f$ i=m,\ldots,1 \f$ and \f$ j=1,\ldots,n \f$.
|
||||
*/
|
||||
template <typename MatrixType>
|
||||
MatrixType matrix_function_solve_triangular_sylvester(const MatrixType& A, const MatrixType& B, const MatrixType& C)
|
||||
{
|
||||
eigen_assert(A.rows() == A.cols());
|
||||
eigen_assert(A.isUpperTriangular());
|
||||
eigen_assert(B.rows() == B.cols());
|
||||
eigen_assert(B.isUpperTriangular());
|
||||
eigen_assert(C.rows() == A.rows());
|
||||
eigen_assert(C.cols() == B.rows());
|
||||
|
||||
typedef typename MatrixType::Index Index;
|
||||
typedef typename MatrixType::Scalar Scalar;
|
||||
|
||||
Index m = A.rows();
|
||||
Index n = B.rows();
|
||||
MatrixType X(m, n);
|
||||
|
||||
for (Index i = m - 1; i >= 0; --i) {
|
||||
for (Index j = 0; j < n; ++j) {
|
||||
|
||||
// Compute AX = \sum_{k=i+1}^m A_{ik} X_{kj}
|
||||
Scalar AX;
|
||||
if (i == m - 1) {
|
||||
AX = 0;
|
||||
} else {
|
||||
Matrix<Scalar,1,1> AXmatrix = A.row(i).tail(m-1-i) * X.col(j).tail(m-1-i);
|
||||
AX = AXmatrix(0,0);
|
||||
}
|
||||
|
||||
// Compute XB = \sum_{k=1}^{j-1} X_{ik} B_{kj}
|
||||
Scalar XB;
|
||||
if (j == 0) {
|
||||
XB = 0;
|
||||
} else {
|
||||
Matrix<Scalar,1,1> XBmatrix = X.row(i).head(j) * B.col(j).head(j);
|
||||
XB = XBmatrix(0,0);
|
||||
}
|
||||
|
||||
X(i,j) = (C(i,j) - AX - XB) / (A(i,i) + B(j,j));
|
||||
}
|
||||
}
|
||||
return X;
|
||||
}
|
||||
|
||||
/** \brief Compute part of matrix function above block diagonal.
|
||||
*
|
||||
* This routine completes the computation of \p fT, denoting a matrix function applied to the triangular
|
||||
* matrix \p T. It assumes that the block diagonal part of \p fT has already been computed. The part below
|
||||
* the diagonal is zero, because \p T is upper triangular.
|
||||
*/
|
||||
template <typename MatrixType, typename VectorType>
|
||||
void matrix_function_compute_above_diagonal(const MatrixType& T, const VectorType& blockStart, const VectorType& clusterSize, MatrixType& fT)
|
||||
{
|
||||
typedef internal::traits<MatrixType> Traits;
|
||||
typedef typename MatrixType::Scalar Scalar;
|
||||
typedef typename MatrixType::Index Index;
|
||||
static const int RowsAtCompileTime = Traits::RowsAtCompileTime;
|
||||
static const int ColsAtCompileTime = Traits::ColsAtCompileTime;
|
||||
static const int Options = MatrixType::Options;
|
||||
typedef Matrix<Scalar, Dynamic, Dynamic, Options, RowsAtCompileTime, ColsAtCompileTime> DynMatrixType;
|
||||
|
||||
for (Index k = 1; k < clusterSize.rows(); k++) {
|
||||
for (Index i = 0; i < clusterSize.rows() - k; i++) {
|
||||
// compute (i, i+k) block
|
||||
DynMatrixType A = T.block(blockStart(i), blockStart(i), clusterSize(i), clusterSize(i));
|
||||
DynMatrixType B = -T.block(blockStart(i+k), blockStart(i+k), clusterSize(i+k), clusterSize(i+k));
|
||||
DynMatrixType C = fT.block(blockStart(i), blockStart(i), clusterSize(i), clusterSize(i))
|
||||
* T.block(blockStart(i), blockStart(i+k), clusterSize(i), clusterSize(i+k));
|
||||
C -= T.block(blockStart(i), blockStart(i+k), clusterSize(i), clusterSize(i+k))
|
||||
* fT.block(blockStart(i+k), blockStart(i+k), clusterSize(i+k), clusterSize(i+k));
|
||||
for (Index m = i + 1; m < i + k; m++) {
|
||||
C += fT.block(blockStart(i), blockStart(m), clusterSize(i), clusterSize(m))
|
||||
* T.block(blockStart(m), blockStart(i+k), clusterSize(m), clusterSize(i+k));
|
||||
C -= T.block(blockStart(i), blockStart(m), clusterSize(i), clusterSize(m))
|
||||
* fT.block(blockStart(m), blockStart(i+k), clusterSize(m), clusterSize(i+k));
|
||||
}
|
||||
fT.block(blockStart(i), blockStart(i+k), clusterSize(i), clusterSize(i+k))
|
||||
= matrix_function_solve_triangular_sylvester(A, B, C);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/** \ingroup MatrixFunctions_Module
|
||||
* \brief Class for computing matrix functions.
|
||||
* \tparam MatrixType type of the argument of the matrix function,
|
||||
* expected to be an instantiation of the Matrix class template.
|
||||
* \tparam AtomicType type for computing matrix function of atomic blocks.
|
||||
* \tparam IsComplex used internally to select correct specialization.
|
||||
*
|
||||
* This class implements the Schur-Parlett algorithm for computing matrix functions. The spectrum of the
|
||||
* matrix is divided in clustered of eigenvalues that lies close together. This class delegates the
|
||||
* computation of the matrix function on every block corresponding to these clusters to an object of type
|
||||
* \p AtomicType and uses these results to compute the matrix function of the whole matrix. The class
|
||||
* \p AtomicType should have a \p compute() member function for computing the matrix function of a block.
|
||||
*
|
||||
* \sa class MatrixFunctionAtomic, class MatrixLogarithmAtomic
|
||||
*/
|
||||
template <typename MatrixType, int IsComplex = NumTraits<typename internal::traits<MatrixType>::Scalar>::IsComplex>
|
||||
struct matrix_function_compute
|
||||
{
|
||||
/** \brief Compute the matrix function.
|
||||
*
|
||||
* \param[in] A argument of matrix function, should be a square matrix.
|
||||
* \param[in] atomic class for computing matrix function of atomic blocks.
|
||||
* \param[out] result the function \p f applied to \p A, as
|
||||
* specified in the constructor.
|
||||
*
|
||||
* See MatrixBase::matrixFunction() for details on how this computation
|
||||
* is implemented.
|
||||
*/
|
||||
template <typename AtomicType, typename ResultType>
|
||||
static void run(const MatrixType& A, AtomicType& atomic, ResultType &result);
|
||||
};
|
||||
|
||||
/** \internal \ingroup MatrixFunctions_Module
|
||||
* \brief Partial specialization of MatrixFunction for real matrices
|
||||
*
|
||||
* This converts the real matrix to a complex matrix, compute the matrix function of that matrix, and then
|
||||
* converts the result back to a real matrix.
|
||||
*/
|
||||
template <typename MatrixType>
|
||||
struct matrix_function_compute<MatrixType, 0>
|
||||
{
|
||||
template <typename MatA, typename AtomicType, typename ResultType>
|
||||
static void run(const MatA& A, AtomicType& atomic, ResultType &result)
|
||||
{
|
||||
typedef internal::traits<MatrixType> Traits;
|
||||
typedef typename Traits::Scalar Scalar;
|
||||
static const int Rows = Traits::RowsAtCompileTime, Cols = Traits::ColsAtCompileTime;
|
||||
static const int MaxRows = Traits::MaxRowsAtCompileTime, MaxCols = Traits::MaxColsAtCompileTime;
|
||||
|
||||
typedef std::complex<Scalar> ComplexScalar;
|
||||
typedef Matrix<ComplexScalar, Rows, Cols, 0, MaxRows, MaxCols> ComplexMatrix;
|
||||
|
||||
ComplexMatrix CA = A.template cast<ComplexScalar>();
|
||||
ComplexMatrix Cresult;
|
||||
matrix_function_compute<ComplexMatrix>::run(CA, atomic, Cresult);
|
||||
result = Cresult.real();
|
||||
}
|
||||
};
|
||||
|
||||
/** \internal \ingroup MatrixFunctions_Module
|
||||
* \brief Partial specialization of MatrixFunction for complex matrices
|
||||
*/
|
||||
template <typename MatrixType>
|
||||
struct matrix_function_compute<MatrixType, 1>
|
||||
{
|
||||
template <typename MatA, typename AtomicType, typename ResultType>
|
||||
static void run(const MatA& A, AtomicType& atomic, ResultType &result)
|
||||
{
|
||||
typedef internal::traits<MatrixType> Traits;
|
||||
|
||||
// compute Schur decomposition of A
|
||||
const ComplexSchur<MatrixType> schurOfA(A);
|
||||
MatrixType T = schurOfA.matrixT();
|
||||
MatrixType U = schurOfA.matrixU();
|
||||
|
||||
// partition eigenvalues into clusters of ei'vals "close" to each other
|
||||
std::list<std::list<Index> > clusters;
|
||||
matrix_function_partition_eigenvalues(T.diagonal(), clusters);
|
||||
|
||||
// compute size of each cluster
|
||||
Matrix<Index, Dynamic, 1> clusterSize;
|
||||
matrix_function_compute_cluster_size(clusters, clusterSize);
|
||||
|
||||
// blockStart[i] is row index at which block corresponding to i-th cluster starts
|
||||
Matrix<Index, Dynamic, 1> blockStart;
|
||||
matrix_function_compute_block_start(clusterSize, blockStart);
|
||||
|
||||
// compute map so that eivalToCluster[i] = j means that i-th ei'val is in j-th cluster
|
||||
Matrix<Index, Dynamic, 1> eivalToCluster;
|
||||
matrix_function_compute_map(T.diagonal(), clusters, eivalToCluster);
|
||||
|
||||
// compute permutation which groups ei'vals in same cluster together
|
||||
Matrix<Index, Traits::RowsAtCompileTime, 1> permutation;
|
||||
matrix_function_compute_permutation(blockStart, eivalToCluster, permutation);
|
||||
|
||||
// permute Schur decomposition
|
||||
matrix_function_permute_schur(permutation, U, T);
|
||||
|
||||
// compute result
|
||||
MatrixType fT; // matrix function applied to T
|
||||
matrix_function_compute_block_atomic(T, atomic, blockStart, clusterSize, fT);
|
||||
matrix_function_compute_above_diagonal(T, blockStart, clusterSize, fT);
|
||||
result = U * (fT.template triangularView<Upper>() * U.adjoint());
|
||||
}
|
||||
};
|
||||
|
||||
} // end of namespace internal
|
||||
|
||||
/** \ingroup MatrixFunctions_Module
|
||||
*
|
||||
* \brief Proxy for the matrix function of some matrix (expression).
|
||||
*
|
||||
* \tparam Derived Type of the argument to the matrix function.
|
||||
*
|
||||
* This class holds the argument to the matrix function until it is assigned or evaluated for some other
|
||||
* reason (so the argument should not be changed in the meantime). It is the return type of
|
||||
* matrixBase::matrixFunction() and related functions and most of the time this is the only way it is used.
|
||||
*/
|
||||
template<typename Derived> class MatrixFunctionReturnValue
|
||||
: public ReturnByValue<MatrixFunctionReturnValue<Derived> >
|
||||
{
|
||||
public:
|
||||
typedef typename Derived::Scalar Scalar;
|
||||
typedef typename Derived::Index Index;
|
||||
typedef typename internal::stem_function<Scalar>::type StemFunction;
|
||||
|
||||
protected:
|
||||
typedef typename internal::ref_selector<Derived>::type DerivedNested;
|
||||
|
||||
public:
|
||||
|
||||
/** \brief Constructor.
|
||||
*
|
||||
* \param[in] A %Matrix (expression) forming the argument of the matrix function.
|
||||
* \param[in] f Stem function for matrix function under consideration.
|
||||
*/
|
||||
MatrixFunctionReturnValue(const Derived& A, StemFunction f) : m_A(A), m_f(f) { }
|
||||
|
||||
/** \brief Compute the matrix function.
|
||||
*
|
||||
* \param[out] result \p f applied to \p A, where \p f and \p A are as in the constructor.
|
||||
*/
|
||||
template <typename ResultType>
|
||||
inline void evalTo(ResultType& result) const
|
||||
{
|
||||
typedef typename internal::nested_eval<Derived, 10>::type NestedEvalType;
|
||||
typedef typename internal::remove_all<NestedEvalType>::type NestedEvalTypeClean;
|
||||
typedef internal::traits<NestedEvalTypeClean> Traits;
|
||||
static const int RowsAtCompileTime = Traits::RowsAtCompileTime;
|
||||
static const int ColsAtCompileTime = Traits::ColsAtCompileTime;
|
||||
typedef std::complex<typename NumTraits<Scalar>::Real> ComplexScalar;
|
||||
typedef Matrix<ComplexScalar, Dynamic, Dynamic, 0, RowsAtCompileTime, ColsAtCompileTime> DynMatrixType;
|
||||
|
||||
typedef internal::MatrixFunctionAtomic<DynMatrixType> AtomicType;
|
||||
AtomicType atomic(m_f);
|
||||
|
||||
internal::matrix_function_compute<typename NestedEvalTypeClean::PlainObject>::run(m_A, atomic, result);
|
||||
}
|
||||
|
||||
Index rows() const { return m_A.rows(); }
|
||||
Index cols() const { return m_A.cols(); }
|
||||
|
||||
private:
|
||||
const DerivedNested m_A;
|
||||
StemFunction *m_f;
|
||||
};
|
||||
|
||||
namespace internal {
|
||||
template<typename Derived>
|
||||
struct traits<MatrixFunctionReturnValue<Derived> >
|
||||
{
|
||||
typedef typename Derived::PlainObject ReturnType;
|
||||
};
|
||||
}
|
||||
|
||||
|
||||
/********** MatrixBase methods **********/
|
||||
|
||||
|
||||
template <typename Derived>
|
||||
const MatrixFunctionReturnValue<Derived> MatrixBase<Derived>::matrixFunction(typename internal::stem_function<typename internal::traits<Derived>::Scalar>::type f) const
|
||||
{
|
||||
eigen_assert(rows() == cols());
|
||||
return MatrixFunctionReturnValue<Derived>(derived(), f);
|
||||
}
|
||||
|
||||
template <typename Derived>
|
||||
const MatrixFunctionReturnValue<Derived> MatrixBase<Derived>::sin() const
|
||||
{
|
||||
eigen_assert(rows() == cols());
|
||||
typedef typename internal::stem_function<Scalar>::ComplexScalar ComplexScalar;
|
||||
return MatrixFunctionReturnValue<Derived>(derived(), internal::stem_function_sin<ComplexScalar>);
|
||||
}
|
||||
|
||||
template <typename Derived>
|
||||
const MatrixFunctionReturnValue<Derived> MatrixBase<Derived>::cos() const
|
||||
{
|
||||
eigen_assert(rows() == cols());
|
||||
typedef typename internal::stem_function<Scalar>::ComplexScalar ComplexScalar;
|
||||
return MatrixFunctionReturnValue<Derived>(derived(), internal::stem_function_cos<ComplexScalar>);
|
||||
}
|
||||
|
||||
template <typename Derived>
|
||||
const MatrixFunctionReturnValue<Derived> MatrixBase<Derived>::sinh() const
|
||||
{
|
||||
eigen_assert(rows() == cols());
|
||||
typedef typename internal::stem_function<Scalar>::ComplexScalar ComplexScalar;
|
||||
return MatrixFunctionReturnValue<Derived>(derived(), internal::stem_function_sinh<ComplexScalar>);
|
||||
}
|
||||
|
||||
template <typename Derived>
|
||||
const MatrixFunctionReturnValue<Derived> MatrixBase<Derived>::cosh() const
|
||||
{
|
||||
eigen_assert(rows() == cols());
|
||||
typedef typename internal::stem_function<Scalar>::ComplexScalar ComplexScalar;
|
||||
return MatrixFunctionReturnValue<Derived>(derived(), internal::stem_function_cosh<ComplexScalar>);
|
||||
}
|
||||
|
||||
} // end namespace Eigen
|
||||
|
||||
#endif // EIGEN_MATRIX_FUNCTION_H
|
||||
@@ -1,373 +0,0 @@
|
||||
// This file is part of Eigen, a lightweight C++ template library
|
||||
// for linear algebra.
|
||||
//
|
||||
// Copyright (C) 2011, 2013 Jitse Niesen <jitse@maths.leeds.ac.uk>
|
||||
// Copyright (C) 2011 Chen-Pang He <jdh8@ms63.hinet.net>
|
||||
//
|
||||
// This Source Code Form is subject to the terms of the Mozilla
|
||||
// Public License v. 2.0. If a copy of the MPL was not distributed
|
||||
// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
|
||||
|
||||
#ifndef EIGEN_MATRIX_LOGARITHM
|
||||
#define EIGEN_MATRIX_LOGARITHM
|
||||
|
||||
namespace Eigen {
|
||||
|
||||
namespace internal {
|
||||
|
||||
template <typename Scalar>
|
||||
struct matrix_log_min_pade_degree
|
||||
{
|
||||
static const int value = 3;
|
||||
};
|
||||
|
||||
template <typename Scalar>
|
||||
struct matrix_log_max_pade_degree
|
||||
{
|
||||
typedef typename NumTraits<Scalar>::Real RealScalar;
|
||||
static const int value = std::numeric_limits<RealScalar>::digits<= 24? 5: // single precision
|
||||
std::numeric_limits<RealScalar>::digits<= 53? 7: // double precision
|
||||
std::numeric_limits<RealScalar>::digits<= 64? 8: // extended precision
|
||||
std::numeric_limits<RealScalar>::digits<=106? 10: // double-double
|
||||
11; // quadruple precision
|
||||
};
|
||||
|
||||
/** \brief Compute logarithm of 2x2 triangular matrix. */
|
||||
template <typename MatrixType>
|
||||
void matrix_log_compute_2x2(const MatrixType& A, MatrixType& result)
|
||||
{
|
||||
typedef typename MatrixType::Scalar Scalar;
|
||||
typedef typename MatrixType::RealScalar RealScalar;
|
||||
using std::abs;
|
||||
using std::ceil;
|
||||
using std::imag;
|
||||
using std::log;
|
||||
|
||||
Scalar logA00 = log(A(0,0));
|
||||
Scalar logA11 = log(A(1,1));
|
||||
|
||||
result(0,0) = logA00;
|
||||
result(1,0) = Scalar(0);
|
||||
result(1,1) = logA11;
|
||||
|
||||
Scalar y = A(1,1) - A(0,0);
|
||||
if (y==Scalar(0))
|
||||
{
|
||||
result(0,1) = A(0,1) / A(0,0);
|
||||
}
|
||||
else if ((abs(A(0,0)) < RealScalar(0.5)*abs(A(1,1))) || (abs(A(0,0)) > 2*abs(A(1,1))))
|
||||
{
|
||||
result(0,1) = A(0,1) * (logA11 - logA00) / y;
|
||||
}
|
||||
else
|
||||
{
|
||||
// computation in previous branch is inaccurate if A(1,1) \approx A(0,0)
|
||||
int unwindingNumber = static_cast<int>(ceil((imag(logA11 - logA00) - RealScalar(EIGEN_PI)) / RealScalar(2*EIGEN_PI)));
|
||||
result(0,1) = A(0,1) * (numext::log1p(y/A(0,0)) + Scalar(0,2*EIGEN_PI*unwindingNumber)) / y;
|
||||
}
|
||||
}
|
||||
|
||||
/* \brief Get suitable degree for Pade approximation. (specialized for RealScalar = float) */
|
||||
inline int matrix_log_get_pade_degree(float normTminusI)
|
||||
{
|
||||
const float maxNormForPade[] = { 2.5111573934555054e-1 /* degree = 3 */ , 4.0535837411880493e-1,
|
||||
5.3149729967117310e-1 };
|
||||
const int minPadeDegree = matrix_log_min_pade_degree<float>::value;
|
||||
const int maxPadeDegree = matrix_log_max_pade_degree<float>::value;
|
||||
int degree = minPadeDegree;
|
||||
for (; degree <= maxPadeDegree; ++degree)
|
||||
if (normTminusI <= maxNormForPade[degree - minPadeDegree])
|
||||
break;
|
||||
return degree;
|
||||
}
|
||||
|
||||
/* \brief Get suitable degree for Pade approximation. (specialized for RealScalar = double) */
|
||||
inline int matrix_log_get_pade_degree(double normTminusI)
|
||||
{
|
||||
const double maxNormForPade[] = { 1.6206284795015624e-2 /* degree = 3 */ , 5.3873532631381171e-2,
|
||||
1.1352802267628681e-1, 1.8662860613541288e-1, 2.642960831111435e-1 };
|
||||
const int minPadeDegree = matrix_log_min_pade_degree<double>::value;
|
||||
const int maxPadeDegree = matrix_log_max_pade_degree<double>::value;
|
||||
int degree = minPadeDegree;
|
||||
for (; degree <= maxPadeDegree; ++degree)
|
||||
if (normTminusI <= maxNormForPade[degree - minPadeDegree])
|
||||
break;
|
||||
return degree;
|
||||
}
|
||||
|
||||
/* \brief Get suitable degree for Pade approximation. (specialized for RealScalar = long double) */
|
||||
inline int matrix_log_get_pade_degree(long double normTminusI)
|
||||
{
|
||||
#if LDBL_MANT_DIG == 53 // double precision
|
||||
const long double maxNormForPade[] = { 1.6206284795015624e-2L /* degree = 3 */ , 5.3873532631381171e-2L,
|
||||
1.1352802267628681e-1L, 1.8662860613541288e-1L, 2.642960831111435e-1L };
|
||||
#elif LDBL_MANT_DIG <= 64 // extended precision
|
||||
const long double maxNormForPade[] = { 5.48256690357782863103e-3L /* degree = 3 */, 2.34559162387971167321e-2L,
|
||||
5.84603923897347449857e-2L, 1.08486423756725170223e-1L, 1.68385767881294446649e-1L,
|
||||
2.32777776523703892094e-1L };
|
||||
#elif LDBL_MANT_DIG <= 106 // double-double
|
||||
const long double maxNormForPade[] = { 8.58970550342939562202529664318890e-5L /* degree = 3 */,
|
||||
9.34074328446359654039446552677759e-4L, 4.26117194647672175773064114582860e-3L,
|
||||
1.21546224740281848743149666560464e-2L, 2.61100544998339436713088248557444e-2L,
|
||||
4.66170074627052749243018566390567e-2L, 7.32585144444135027565872014932387e-2L,
|
||||
1.05026503471351080481093652651105e-1L };
|
||||
#else // quadruple precision
|
||||
const long double maxNormForPade[] = { 4.7419931187193005048501568167858103e-5L /* degree = 3 */,
|
||||
5.8853168473544560470387769480192666e-4L, 2.9216120366601315391789493628113520e-3L,
|
||||
8.8415758124319434347116734705174308e-3L, 1.9850836029449446668518049562565291e-2L,
|
||||
3.6688019729653446926585242192447447e-2L, 5.9290962294020186998954055264528393e-2L,
|
||||
8.6998436081634343903250580992127677e-2L, 1.1880960220216759245467951592883642e-1L };
|
||||
#endif
|
||||
const int minPadeDegree = matrix_log_min_pade_degree<long double>::value;
|
||||
const int maxPadeDegree = matrix_log_max_pade_degree<long double>::value;
|
||||
int degree = minPadeDegree;
|
||||
for (; degree <= maxPadeDegree; ++degree)
|
||||
if (normTminusI <= maxNormForPade[degree - minPadeDegree])
|
||||
break;
|
||||
return degree;
|
||||
}
|
||||
|
||||
/* \brief Compute Pade approximation to matrix logarithm */
|
||||
template <typename MatrixType>
|
||||
void matrix_log_compute_pade(MatrixType& result, const MatrixType& T, int degree)
|
||||
{
|
||||
typedef typename NumTraits<typename MatrixType::Scalar>::Real RealScalar;
|
||||
const int minPadeDegree = 3;
|
||||
const int maxPadeDegree = 11;
|
||||
assert(degree >= minPadeDegree && degree <= maxPadeDegree);
|
||||
|
||||
const RealScalar nodes[][maxPadeDegree] = {
|
||||
{ 0.1127016653792583114820734600217600L, 0.5000000000000000000000000000000000L, // degree 3
|
||||
0.8872983346207416885179265399782400L },
|
||||
{ 0.0694318442029737123880267555535953L, 0.3300094782075718675986671204483777L, // degree 4
|
||||
0.6699905217924281324013328795516223L, 0.9305681557970262876119732444464048L },
|
||||
{ 0.0469100770306680036011865608503035L, 0.2307653449471584544818427896498956L, // degree 5
|
||||
0.5000000000000000000000000000000000L, 0.7692346550528415455181572103501044L,
|
||||
0.9530899229693319963988134391496965L },
|
||||
{ 0.0337652428984239860938492227530027L, 0.1693953067668677431693002024900473L, // degree 6
|
||||
0.3806904069584015456847491391596440L, 0.6193095930415984543152508608403560L,
|
||||
0.8306046932331322568306997975099527L, 0.9662347571015760139061507772469973L },
|
||||
{ 0.0254460438286207377369051579760744L, 0.1292344072003027800680676133596058L, // degree 7
|
||||
0.2970774243113014165466967939615193L, 0.5000000000000000000000000000000000L,
|
||||
0.7029225756886985834533032060384807L, 0.8707655927996972199319323866403942L,
|
||||
0.9745539561713792622630948420239256L },
|
||||
{ 0.0198550717512318841582195657152635L, 0.1016667612931866302042230317620848L, // degree 8
|
||||
0.2372337950418355070911304754053768L, 0.4082826787521750975302619288199080L,
|
||||
0.5917173212478249024697380711800920L, 0.7627662049581644929088695245946232L,
|
||||
0.8983332387068133697957769682379152L, 0.9801449282487681158417804342847365L },
|
||||
{ 0.0159198802461869550822118985481636L, 0.0819844463366821028502851059651326L, // degree 9
|
||||
0.1933142836497048013456489803292629L, 0.3378732882980955354807309926783317L,
|
||||
0.5000000000000000000000000000000000L, 0.6621267117019044645192690073216683L,
|
||||
0.8066857163502951986543510196707371L, 0.9180155536633178971497148940348674L,
|
||||
0.9840801197538130449177881014518364L },
|
||||
{ 0.0130467357414141399610179939577740L, 0.0674683166555077446339516557882535L, // degree 10
|
||||
0.1602952158504877968828363174425632L, 0.2833023029353764046003670284171079L,
|
||||
0.4255628305091843945575869994351400L, 0.5744371694908156054424130005648600L,
|
||||
0.7166976970646235953996329715828921L, 0.8397047841495122031171636825574368L,
|
||||
0.9325316833444922553660483442117465L, 0.9869532642585858600389820060422260L },
|
||||
{ 0.0108856709269715035980309994385713L, 0.0564687001159523504624211153480364L, // degree 11
|
||||
0.1349239972129753379532918739844233L, 0.2404519353965940920371371652706952L,
|
||||
0.3652284220238275138342340072995692L, 0.5000000000000000000000000000000000L,
|
||||
0.6347715779761724861657659927004308L, 0.7595480646034059079628628347293048L,
|
||||
0.8650760027870246620467081260155767L, 0.9435312998840476495375788846519636L,
|
||||
0.9891143290730284964019690005614287L } };
|
||||
|
||||
const RealScalar weights[][maxPadeDegree] = {
|
||||
{ 0.2777777777777777777777777777777778L, 0.4444444444444444444444444444444444L, // degree 3
|
||||
0.2777777777777777777777777777777778L },
|
||||
{ 0.1739274225687269286865319746109997L, 0.3260725774312730713134680253890003L, // degree 4
|
||||
0.3260725774312730713134680253890003L, 0.1739274225687269286865319746109997L },
|
||||
{ 0.1184634425280945437571320203599587L, 0.2393143352496832340206457574178191L, // degree 5
|
||||
0.2844444444444444444444444444444444L, 0.2393143352496832340206457574178191L,
|
||||
0.1184634425280945437571320203599587L },
|
||||
{ 0.0856622461895851725201480710863665L, 0.1803807865240693037849167569188581L, // degree 6
|
||||
0.2339569672863455236949351719947755L, 0.2339569672863455236949351719947755L,
|
||||
0.1803807865240693037849167569188581L, 0.0856622461895851725201480710863665L },
|
||||
{ 0.0647424830844348466353057163395410L, 0.1398526957446383339507338857118898L, // degree 7
|
||||
0.1909150252525594724751848877444876L, 0.2089795918367346938775510204081633L,
|
||||
0.1909150252525594724751848877444876L, 0.1398526957446383339507338857118898L,
|
||||
0.0647424830844348466353057163395410L },
|
||||
{ 0.0506142681451881295762656771549811L, 0.1111905172266872352721779972131204L, // degree 8
|
||||
0.1568533229389436436689811009933007L, 0.1813418916891809914825752246385978L,
|
||||
0.1813418916891809914825752246385978L, 0.1568533229389436436689811009933007L,
|
||||
0.1111905172266872352721779972131204L, 0.0506142681451881295762656771549811L },
|
||||
{ 0.0406371941807872059859460790552618L, 0.0903240803474287020292360156214564L, // degree 9
|
||||
0.1303053482014677311593714347093164L, 0.1561735385200014200343152032922218L,
|
||||
0.1651196775006298815822625346434870L, 0.1561735385200014200343152032922218L,
|
||||
0.1303053482014677311593714347093164L, 0.0903240803474287020292360156214564L,
|
||||
0.0406371941807872059859460790552618L },
|
||||
{ 0.0333356721543440687967844049466659L, 0.0747256745752902965728881698288487L, // degree 10
|
||||
0.1095431812579910219977674671140816L, 0.1346333596549981775456134607847347L,
|
||||
0.1477621123573764350869464973256692L, 0.1477621123573764350869464973256692L,
|
||||
0.1346333596549981775456134607847347L, 0.1095431812579910219977674671140816L,
|
||||
0.0747256745752902965728881698288487L, 0.0333356721543440687967844049466659L },
|
||||
{ 0.0278342835580868332413768602212743L, 0.0627901847324523123173471496119701L, // degree 11
|
||||
0.0931451054638671257130488207158280L, 0.1165968822959952399592618524215876L,
|
||||
0.1314022722551233310903444349452546L, 0.1364625433889503153572417641681711L,
|
||||
0.1314022722551233310903444349452546L, 0.1165968822959952399592618524215876L,
|
||||
0.0931451054638671257130488207158280L, 0.0627901847324523123173471496119701L,
|
||||
0.0278342835580868332413768602212743L } };
|
||||
|
||||
MatrixType TminusI = T - MatrixType::Identity(T.rows(), T.rows());
|
||||
result.setZero(T.rows(), T.rows());
|
||||
for (int k = 0; k < degree; ++k) {
|
||||
RealScalar weight = weights[degree-minPadeDegree][k];
|
||||
RealScalar node = nodes[degree-minPadeDegree][k];
|
||||
result += weight * (MatrixType::Identity(T.rows(), T.rows()) + node * TminusI)
|
||||
.template triangularView<Upper>().solve(TminusI);
|
||||
}
|
||||
}
|
||||
|
||||
/** \brief Compute logarithm of triangular matrices with size > 2.
|
||||
* \details This uses a inverse scale-and-square algorithm. */
|
||||
template <typename MatrixType>
|
||||
void matrix_log_compute_big(const MatrixType& A, MatrixType& result)
|
||||
{
|
||||
typedef typename MatrixType::Scalar Scalar;
|
||||
typedef typename NumTraits<Scalar>::Real RealScalar;
|
||||
using std::pow;
|
||||
|
||||
int numberOfSquareRoots = 0;
|
||||
int numberOfExtraSquareRoots = 0;
|
||||
int degree;
|
||||
MatrixType T = A, sqrtT;
|
||||
|
||||
int maxPadeDegree = matrix_log_max_pade_degree<Scalar>::value;
|
||||
const RealScalar maxNormForPade = maxPadeDegree<= 5? 5.3149729967117310e-1L: // single precision
|
||||
maxPadeDegree<= 7? 2.6429608311114350e-1L: // double precision
|
||||
maxPadeDegree<= 8? 2.32777776523703892094e-1L: // extended precision
|
||||
maxPadeDegree<=10? 1.05026503471351080481093652651105e-1L: // double-double
|
||||
1.1880960220216759245467951592883642e-1L; // quadruple precision
|
||||
|
||||
while (true) {
|
||||
RealScalar normTminusI = (T - MatrixType::Identity(T.rows(), T.rows())).cwiseAbs().colwise().sum().maxCoeff();
|
||||
if (normTminusI < maxNormForPade) {
|
||||
degree = matrix_log_get_pade_degree(normTminusI);
|
||||
int degree2 = matrix_log_get_pade_degree(normTminusI / RealScalar(2));
|
||||
if ((degree - degree2 <= 1) || (numberOfExtraSquareRoots == 1))
|
||||
break;
|
||||
++numberOfExtraSquareRoots;
|
||||
}
|
||||
matrix_sqrt_triangular(T, sqrtT);
|
||||
T = sqrtT.template triangularView<Upper>();
|
||||
++numberOfSquareRoots;
|
||||
}
|
||||
|
||||
matrix_log_compute_pade(result, T, degree);
|
||||
result *= pow(RealScalar(2), numberOfSquareRoots);
|
||||
}
|
||||
|
||||
/** \ingroup MatrixFunctions_Module
|
||||
* \class MatrixLogarithmAtomic
|
||||
* \brief Helper class for computing matrix logarithm of atomic matrices.
|
||||
*
|
||||
* Here, an atomic matrix is a triangular matrix whose diagonal entries are close to each other.
|
||||
*
|
||||
* \sa class MatrixFunctionAtomic, MatrixBase::log()
|
||||
*/
|
||||
template <typename MatrixType>
|
||||
class MatrixLogarithmAtomic
|
||||
{
|
||||
public:
|
||||
/** \brief Compute matrix logarithm of atomic matrix
|
||||
* \param[in] A argument of matrix logarithm, should be upper triangular and atomic
|
||||
* \returns The logarithm of \p A.
|
||||
*/
|
||||
MatrixType compute(const MatrixType& A);
|
||||
};
|
||||
|
||||
template <typename MatrixType>
|
||||
MatrixType MatrixLogarithmAtomic<MatrixType>::compute(const MatrixType& A)
|
||||
{
|
||||
using std::log;
|
||||
MatrixType result(A.rows(), A.rows());
|
||||
if (A.rows() == 1)
|
||||
result(0,0) = log(A(0,0));
|
||||
else if (A.rows() == 2)
|
||||
matrix_log_compute_2x2(A, result);
|
||||
else
|
||||
matrix_log_compute_big(A, result);
|
||||
return result;
|
||||
}
|
||||
|
||||
} // end of namespace internal
|
||||
|
||||
/** \ingroup MatrixFunctions_Module
|
||||
*
|
||||
* \brief Proxy for the matrix logarithm of some matrix (expression).
|
||||
*
|
||||
* \tparam Derived Type of the argument to the matrix function.
|
||||
*
|
||||
* This class holds the argument to the matrix function until it is
|
||||
* assigned or evaluated for some other reason (so the argument
|
||||
* should not be changed in the meantime). It is the return type of
|
||||
* MatrixBase::log() and most of the time this is the only way it
|
||||
* is used.
|
||||
*/
|
||||
template<typename Derived> class MatrixLogarithmReturnValue
|
||||
: public ReturnByValue<MatrixLogarithmReturnValue<Derived> >
|
||||
{
|
||||
public:
|
||||
typedef typename Derived::Scalar Scalar;
|
||||
typedef typename Derived::Index Index;
|
||||
|
||||
protected:
|
||||
typedef typename internal::ref_selector<Derived>::type DerivedNested;
|
||||
|
||||
public:
|
||||
|
||||
/** \brief Constructor.
|
||||
*
|
||||
* \param[in] A %Matrix (expression) forming the argument of the matrix logarithm.
|
||||
*/
|
||||
explicit MatrixLogarithmReturnValue(const Derived& A) : m_A(A) { }
|
||||
|
||||
/** \brief Compute the matrix logarithm.
|
||||
*
|
||||
* \param[out] result Logarithm of \c A, where \c A is as specified in the constructor.
|
||||
*/
|
||||
template <typename ResultType>
|
||||
inline void evalTo(ResultType& result) const
|
||||
{
|
||||
typedef typename internal::nested_eval<Derived, 10>::type DerivedEvalType;
|
||||
typedef typename internal::remove_all<DerivedEvalType>::type DerivedEvalTypeClean;
|
||||
typedef internal::traits<DerivedEvalTypeClean> Traits;
|
||||
static const int RowsAtCompileTime = Traits::RowsAtCompileTime;
|
||||
static const int ColsAtCompileTime = Traits::ColsAtCompileTime;
|
||||
typedef std::complex<typename NumTraits<Scalar>::Real> ComplexScalar;
|
||||
typedef Matrix<ComplexScalar, Dynamic, Dynamic, 0, RowsAtCompileTime, ColsAtCompileTime> DynMatrixType;
|
||||
typedef internal::MatrixLogarithmAtomic<DynMatrixType> AtomicType;
|
||||
AtomicType atomic;
|
||||
|
||||
internal::matrix_function_compute<typename DerivedEvalTypeClean::PlainObject>::run(m_A, atomic, result);
|
||||
}
|
||||
|
||||
Index rows() const { return m_A.rows(); }
|
||||
Index cols() const { return m_A.cols(); }
|
||||
|
||||
private:
|
||||
const DerivedNested m_A;
|
||||
};
|
||||
|
||||
namespace internal {
|
||||
template<typename Derived>
|
||||
struct traits<MatrixLogarithmReturnValue<Derived> >
|
||||
{
|
||||
typedef typename Derived::PlainObject ReturnType;
|
||||
};
|
||||
}
|
||||
|
||||
|
||||
/********** MatrixBase method **********/
|
||||
|
||||
|
||||
template <typename Derived>
|
||||
const MatrixLogarithmReturnValue<Derived> MatrixBase<Derived>::log() const
|
||||
{
|
||||
eigen_assert(rows() == cols());
|
||||
return MatrixLogarithmReturnValue<Derived>(derived());
|
||||
}
|
||||
|
||||
} // end namespace Eigen
|
||||
|
||||
#endif // EIGEN_MATRIX_LOGARITHM
|
||||
@@ -1,709 +0,0 @@
|
||||
// This file is part of Eigen, a lightweight C++ template library
|
||||
// for linear algebra.
|
||||
//
|
||||
// Copyright (C) 2012, 2013 Chen-Pang He <jdh8@ms63.hinet.net>
|
||||
//
|
||||
// This Source Code Form is subject to the terms of the Mozilla
|
||||
// Public License v. 2.0. If a copy of the MPL was not distributed
|
||||
// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
|
||||
|
||||
#ifndef EIGEN_MATRIX_POWER
|
||||
#define EIGEN_MATRIX_POWER
|
||||
|
||||
namespace Eigen {
|
||||
|
||||
template<typename MatrixType> class MatrixPower;
|
||||
|
||||
/**
|
||||
* \ingroup MatrixFunctions_Module
|
||||
*
|
||||
* \brief Proxy for the matrix power of some matrix.
|
||||
*
|
||||
* \tparam MatrixType type of the base, a matrix.
|
||||
*
|
||||
* This class holds the arguments to the matrix power until it is
|
||||
* assigned or evaluated for some other reason (so the argument
|
||||
* should not be changed in the meantime). It is the return type of
|
||||
* MatrixPower::operator() and related functions and most of the
|
||||
* time this is the only way it is used.
|
||||
*/
|
||||
/* TODO This class is only used by MatrixPower, so it should be nested
|
||||
* into MatrixPower, like MatrixPower::ReturnValue. However, my
|
||||
* compiler complained about unused template parameter in the
|
||||
* following declaration in namespace internal.
|
||||
*
|
||||
* template<typename MatrixType>
|
||||
* struct traits<MatrixPower<MatrixType>::ReturnValue>;
|
||||
*/
|
||||
template<typename MatrixType>
|
||||
class MatrixPowerParenthesesReturnValue : public ReturnByValue< MatrixPowerParenthesesReturnValue<MatrixType> >
|
||||
{
|
||||
public:
|
||||
typedef typename MatrixType::RealScalar RealScalar;
|
||||
typedef typename MatrixType::Index Index;
|
||||
|
||||
/**
|
||||
* \brief Constructor.
|
||||
*
|
||||
* \param[in] pow %MatrixPower storing the base.
|
||||
* \param[in] p scalar, the exponent of the matrix power.
|
||||
*/
|
||||
MatrixPowerParenthesesReturnValue(MatrixPower<MatrixType>& pow, RealScalar p) : m_pow(pow), m_p(p)
|
||||
{ }
|
||||
|
||||
/**
|
||||
* \brief Compute the matrix power.
|
||||
*
|
||||
* \param[out] result
|
||||
*/
|
||||
template<typename ResultType>
|
||||
inline void evalTo(ResultType& result) const
|
||||
{ m_pow.compute(result, m_p); }
|
||||
|
||||
Index rows() const { return m_pow.rows(); }
|
||||
Index cols() const { return m_pow.cols(); }
|
||||
|
||||
private:
|
||||
MatrixPower<MatrixType>& m_pow;
|
||||
const RealScalar m_p;
|
||||
};
|
||||
|
||||
/**
|
||||
* \ingroup MatrixFunctions_Module
|
||||
*
|
||||
* \brief Class for computing matrix powers.
|
||||
*
|
||||
* \tparam MatrixType type of the base, expected to be an instantiation
|
||||
* of the Matrix class template.
|
||||
*
|
||||
* This class is capable of computing triangular real/complex matrices
|
||||
* raised to a power in the interval \f$ (-1, 1) \f$.
|
||||
*
|
||||
* \note Currently this class is only used by MatrixPower. One may
|
||||
* insist that this be nested into MatrixPower. This class is here to
|
||||
* faciliate future development of triangular matrix functions.
|
||||
*/
|
||||
template<typename MatrixType>
|
||||
class MatrixPowerAtomic : internal::noncopyable
|
||||
{
|
||||
private:
|
||||
enum {
|
||||
RowsAtCompileTime = MatrixType::RowsAtCompileTime,
|
||||
MaxRowsAtCompileTime = MatrixType::MaxRowsAtCompileTime
|
||||
};
|
||||
typedef typename MatrixType::Scalar Scalar;
|
||||
typedef typename MatrixType::RealScalar RealScalar;
|
||||
typedef std::complex<RealScalar> ComplexScalar;
|
||||
typedef typename MatrixType::Index Index;
|
||||
typedef Block<MatrixType,Dynamic,Dynamic> ResultType;
|
||||
|
||||
const MatrixType& m_A;
|
||||
RealScalar m_p;
|
||||
|
||||
void computePade(int degree, const MatrixType& IminusT, ResultType& res) const;
|
||||
void compute2x2(ResultType& res, RealScalar p) const;
|
||||
void computeBig(ResultType& res) const;
|
||||
static int getPadeDegree(float normIminusT);
|
||||
static int getPadeDegree(double normIminusT);
|
||||
static int getPadeDegree(long double normIminusT);
|
||||
static ComplexScalar computeSuperDiag(const ComplexScalar&, const ComplexScalar&, RealScalar p);
|
||||
static RealScalar computeSuperDiag(RealScalar, RealScalar, RealScalar p);
|
||||
|
||||
public:
|
||||
/**
|
||||
* \brief Constructor.
|
||||
*
|
||||
* \param[in] T the base of the matrix power.
|
||||
* \param[in] p the exponent of the matrix power, should be in
|
||||
* \f$ (-1, 1) \f$.
|
||||
*
|
||||
* The class stores a reference to T, so it should not be changed
|
||||
* (or destroyed) before evaluation. Only the upper triangular
|
||||
* part of T is read.
|
||||
*/
|
||||
MatrixPowerAtomic(const MatrixType& T, RealScalar p);
|
||||
|
||||
/**
|
||||
* \brief Compute the matrix power.
|
||||
*
|
||||
* \param[out] res \f$ A^p \f$ where A and p are specified in the
|
||||
* constructor.
|
||||
*/
|
||||
void compute(ResultType& res) const;
|
||||
};
|
||||
|
||||
template<typename MatrixType>
|
||||
MatrixPowerAtomic<MatrixType>::MatrixPowerAtomic(const MatrixType& T, RealScalar p) :
|
||||
m_A(T), m_p(p)
|
||||
{
|
||||
eigen_assert(T.rows() == T.cols());
|
||||
eigen_assert(p > -1 && p < 1);
|
||||
}
|
||||
|
||||
template<typename MatrixType>
|
||||
void MatrixPowerAtomic<MatrixType>::compute(ResultType& res) const
|
||||
{
|
||||
using std::pow;
|
||||
switch (m_A.rows()) {
|
||||
case 0:
|
||||
break;
|
||||
case 1:
|
||||
res(0,0) = pow(m_A(0,0), m_p);
|
||||
break;
|
||||
case 2:
|
||||
compute2x2(res, m_p);
|
||||
break;
|
||||
default:
|
||||
computeBig(res);
|
||||
}
|
||||
}
|
||||
|
||||
template<typename MatrixType>
|
||||
void MatrixPowerAtomic<MatrixType>::computePade(int degree, const MatrixType& IminusT, ResultType& res) const
|
||||
{
|
||||
int i = 2*degree;
|
||||
res = (m_p-degree) / (2*i-2) * IminusT;
|
||||
|
||||
for (--i; i; --i) {
|
||||
res = (MatrixType::Identity(IminusT.rows(), IminusT.cols()) + res).template triangularView<Upper>()
|
||||
.solve((i==1 ? -m_p : i&1 ? (-m_p-i/2)/(2*i) : (m_p-i/2)/(2*i-2)) * IminusT).eval();
|
||||
}
|
||||
res += MatrixType::Identity(IminusT.rows(), IminusT.cols());
|
||||
}
|
||||
|
||||
// This function assumes that res has the correct size (see bug 614)
|
||||
template<typename MatrixType>
|
||||
void MatrixPowerAtomic<MatrixType>::compute2x2(ResultType& res, RealScalar p) const
|
||||
{
|
||||
using std::abs;
|
||||
using std::pow;
|
||||
res.coeffRef(0,0) = pow(m_A.coeff(0,0), p);
|
||||
|
||||
for (Index i=1; i < m_A.cols(); ++i) {
|
||||
res.coeffRef(i,i) = pow(m_A.coeff(i,i), p);
|
||||
if (m_A.coeff(i-1,i-1) == m_A.coeff(i,i))
|
||||
res.coeffRef(i-1,i) = p * pow(m_A.coeff(i,i), p-1);
|
||||
else if (2*abs(m_A.coeff(i-1,i-1)) < abs(m_A.coeff(i,i)) || 2*abs(m_A.coeff(i,i)) < abs(m_A.coeff(i-1,i-1)))
|
||||
res.coeffRef(i-1,i) = (res.coeff(i,i)-res.coeff(i-1,i-1)) / (m_A.coeff(i,i)-m_A.coeff(i-1,i-1));
|
||||
else
|
||||
res.coeffRef(i-1,i) = computeSuperDiag(m_A.coeff(i,i), m_A.coeff(i-1,i-1), p);
|
||||
res.coeffRef(i-1,i) *= m_A.coeff(i-1,i);
|
||||
}
|
||||
}
|
||||
|
||||
template<typename MatrixType>
|
||||
void MatrixPowerAtomic<MatrixType>::computeBig(ResultType& res) const
|
||||
{
|
||||
using std::ldexp;
|
||||
const int digits = std::numeric_limits<RealScalar>::digits;
|
||||
const RealScalar maxNormForPade = digits <= 24? 4.3386528e-1L // single precision
|
||||
: digits <= 53? 2.789358995219730e-1L // double precision
|
||||
: digits <= 64? 2.4471944416607995472e-1L // extended precision
|
||||
: digits <= 106? 1.1016843812851143391275867258512e-1L // double-double
|
||||
: 9.134603732914548552537150753385375e-2L; // quadruple precision
|
||||
MatrixType IminusT, sqrtT, T = m_A.template triangularView<Upper>();
|
||||
RealScalar normIminusT;
|
||||
int degree, degree2, numberOfSquareRoots = 0;
|
||||
bool hasExtraSquareRoot = false;
|
||||
|
||||
for (Index i=0; i < m_A.cols(); ++i)
|
||||
eigen_assert(m_A(i,i) != RealScalar(0));
|
||||
|
||||
while (true) {
|
||||
IminusT = MatrixType::Identity(m_A.rows(), m_A.cols()) - T;
|
||||
normIminusT = IminusT.cwiseAbs().colwise().sum().maxCoeff();
|
||||
if (normIminusT < maxNormForPade) {
|
||||
degree = getPadeDegree(normIminusT);
|
||||
degree2 = getPadeDegree(normIminusT/2);
|
||||
if (degree - degree2 <= 1 || hasExtraSquareRoot)
|
||||
break;
|
||||
hasExtraSquareRoot = true;
|
||||
}
|
||||
matrix_sqrt_triangular(T, sqrtT);
|
||||
T = sqrtT.template triangularView<Upper>();
|
||||
++numberOfSquareRoots;
|
||||
}
|
||||
computePade(degree, IminusT, res);
|
||||
|
||||
for (; numberOfSquareRoots; --numberOfSquareRoots) {
|
||||
compute2x2(res, ldexp(m_p, -numberOfSquareRoots));
|
||||
res = res.template triangularView<Upper>() * res;
|
||||
}
|
||||
compute2x2(res, m_p);
|
||||
}
|
||||
|
||||
template<typename MatrixType>
|
||||
inline int MatrixPowerAtomic<MatrixType>::getPadeDegree(float normIminusT)
|
||||
{
|
||||
const float maxNormForPade[] = { 2.8064004e-1f /* degree = 3 */ , 4.3386528e-1f };
|
||||
int degree = 3;
|
||||
for (; degree <= 4; ++degree)
|
||||
if (normIminusT <= maxNormForPade[degree - 3])
|
||||
break;
|
||||
return degree;
|
||||
}
|
||||
|
||||
template<typename MatrixType>
|
||||
inline int MatrixPowerAtomic<MatrixType>::getPadeDegree(double normIminusT)
|
||||
{
|
||||
const double maxNormForPade[] = { 1.884160592658218e-2 /* degree = 3 */ , 6.038881904059573e-2, 1.239917516308172e-1,
|
||||
1.999045567181744e-1, 2.789358995219730e-1 };
|
||||
int degree = 3;
|
||||
for (; degree <= 7; ++degree)
|
||||
if (normIminusT <= maxNormForPade[degree - 3])
|
||||
break;
|
||||
return degree;
|
||||
}
|
||||
|
||||
template<typename MatrixType>
|
||||
inline int MatrixPowerAtomic<MatrixType>::getPadeDegree(long double normIminusT)
|
||||
{
|
||||
#if LDBL_MANT_DIG == 53
|
||||
const int maxPadeDegree = 7;
|
||||
const double maxNormForPade[] = { 1.884160592658218e-2L /* degree = 3 */ , 6.038881904059573e-2L, 1.239917516308172e-1L,
|
||||
1.999045567181744e-1L, 2.789358995219730e-1L };
|
||||
#elif LDBL_MANT_DIG <= 64
|
||||
const int maxPadeDegree = 8;
|
||||
const long double maxNormForPade[] = { 6.3854693117491799460e-3L /* degree = 3 */ , 2.6394893435456973676e-2L,
|
||||
6.4216043030404063729e-2L, 1.1701165502926694307e-1L, 1.7904284231268670284e-1L, 2.4471944416607995472e-1L };
|
||||
#elif LDBL_MANT_DIG <= 106
|
||||
const int maxPadeDegree = 10;
|
||||
const double maxNormForPade[] = { 1.0007161601787493236741409687186e-4L /* degree = 3 */ ,
|
||||
1.0007161601787493236741409687186e-3L, 4.7069769360887572939882574746264e-3L, 1.3220386624169159689406653101695e-2L,
|
||||
2.8063482381631737920612944054906e-2L, 4.9625993951953473052385361085058e-2L, 7.7367040706027886224557538328171e-2L,
|
||||
1.1016843812851143391275867258512e-1L };
|
||||
#else
|
||||
const int maxPadeDegree = 10;
|
||||
const double maxNormForPade[] = { 5.524506147036624377378713555116378e-5L /* degree = 3 */ ,
|
||||
6.640600568157479679823602193345995e-4L, 3.227716520106894279249709728084626e-3L,
|
||||
9.619593944683432960546978734646284e-3L, 2.134595382433742403911124458161147e-2L,
|
||||
3.908166513900489428442993794761185e-2L, 6.266780814639442865832535460550138e-2L,
|
||||
9.134603732914548552537150753385375e-2L };
|
||||
#endif
|
||||
int degree = 3;
|
||||
for (; degree <= maxPadeDegree; ++degree)
|
||||
if (normIminusT <= maxNormForPade[degree - 3])
|
||||
break;
|
||||
return degree;
|
||||
}
|
||||
|
||||
template<typename MatrixType>
|
||||
inline typename MatrixPowerAtomic<MatrixType>::ComplexScalar
|
||||
MatrixPowerAtomic<MatrixType>::computeSuperDiag(const ComplexScalar& curr, const ComplexScalar& prev, RealScalar p)
|
||||
{
|
||||
using std::ceil;
|
||||
using std::exp;
|
||||
using std::log;
|
||||
using std::sinh;
|
||||
|
||||
ComplexScalar logCurr = log(curr);
|
||||
ComplexScalar logPrev = log(prev);
|
||||
int unwindingNumber = ceil((numext::imag(logCurr - logPrev) - RealScalar(EIGEN_PI)) / RealScalar(2*EIGEN_PI));
|
||||
ComplexScalar w = numext::log1p((curr-prev)/prev)/RealScalar(2) + ComplexScalar(0, EIGEN_PI*unwindingNumber);
|
||||
return RealScalar(2) * exp(RealScalar(0.5) * p * (logCurr + logPrev)) * sinh(p * w) / (curr - prev);
|
||||
}
|
||||
|
||||
template<typename MatrixType>
|
||||
inline typename MatrixPowerAtomic<MatrixType>::RealScalar
|
||||
MatrixPowerAtomic<MatrixType>::computeSuperDiag(RealScalar curr, RealScalar prev, RealScalar p)
|
||||
{
|
||||
using std::exp;
|
||||
using std::log;
|
||||
using std::sinh;
|
||||
|
||||
RealScalar w = numext::log1p((curr-prev)/prev)/RealScalar(2);
|
||||
return 2 * exp(p * (log(curr) + log(prev)) / 2) * sinh(p * w) / (curr - prev);
|
||||
}
|
||||
|
||||
/**
|
||||
* \ingroup MatrixFunctions_Module
|
||||
*
|
||||
* \brief Class for computing matrix powers.
|
||||
*
|
||||
* \tparam MatrixType type of the base, expected to be an instantiation
|
||||
* of the Matrix class template.
|
||||
*
|
||||
* This class is capable of computing real/complex matrices raised to
|
||||
* an arbitrary real power. Meanwhile, it saves the result of Schur
|
||||
* decomposition if an non-integral power has even been calculated.
|
||||
* Therefore, if you want to compute multiple (>= 2) matrix powers
|
||||
* for the same matrix, using the class directly is more efficient than
|
||||
* calling MatrixBase::pow().
|
||||
*
|
||||
* Example:
|
||||
* \include MatrixPower_optimal.cpp
|
||||
* Output: \verbinclude MatrixPower_optimal.out
|
||||
*/
|
||||
template<typename MatrixType>
|
||||
class MatrixPower : internal::noncopyable
|
||||
{
|
||||
private:
|
||||
typedef typename MatrixType::Scalar Scalar;
|
||||
typedef typename MatrixType::RealScalar RealScalar;
|
||||
typedef typename MatrixType::Index Index;
|
||||
|
||||
public:
|
||||
/**
|
||||
* \brief Constructor.
|
||||
*
|
||||
* \param[in] A the base of the matrix power.
|
||||
*
|
||||
* The class stores a reference to A, so it should not be changed
|
||||
* (or destroyed) before evaluation.
|
||||
*/
|
||||
explicit MatrixPower(const MatrixType& A) :
|
||||
m_A(A),
|
||||
m_conditionNumber(0),
|
||||
m_rank(A.cols()),
|
||||
m_nulls(0)
|
||||
{ eigen_assert(A.rows() == A.cols()); }
|
||||
|
||||
/**
|
||||
* \brief Returns the matrix power.
|
||||
*
|
||||
* \param[in] p exponent, a real scalar.
|
||||
* \return The expression \f$ A^p \f$, where A is specified in the
|
||||
* constructor.
|
||||
*/
|
||||
const MatrixPowerParenthesesReturnValue<MatrixType> operator()(RealScalar p)
|
||||
{ return MatrixPowerParenthesesReturnValue<MatrixType>(*this, p); }
|
||||
|
||||
/**
|
||||
* \brief Compute the matrix power.
|
||||
*
|
||||
* \param[in] p exponent, a real scalar.
|
||||
* \param[out] res \f$ A^p \f$ where A is specified in the
|
||||
* constructor.
|
||||
*/
|
||||
template<typename ResultType>
|
||||
void compute(ResultType& res, RealScalar p);
|
||||
|
||||
Index rows() const { return m_A.rows(); }
|
||||
Index cols() const { return m_A.cols(); }
|
||||
|
||||
private:
|
||||
typedef std::complex<RealScalar> ComplexScalar;
|
||||
typedef Matrix<ComplexScalar, Dynamic, Dynamic, 0,
|
||||
MatrixType::RowsAtCompileTime, MatrixType::ColsAtCompileTime> ComplexMatrix;
|
||||
|
||||
/** \brief Reference to the base of matrix power. */
|
||||
typename MatrixType::Nested m_A;
|
||||
|
||||
/** \brief Temporary storage. */
|
||||
MatrixType m_tmp;
|
||||
|
||||
/** \brief Store the result of Schur decomposition. */
|
||||
ComplexMatrix m_T, m_U;
|
||||
|
||||
/** \brief Store fractional power of m_T. */
|
||||
ComplexMatrix m_fT;
|
||||
|
||||
/**
|
||||
* \brief Condition number of m_A.
|
||||
*
|
||||
* It is initialized as 0 to avoid performing unnecessary Schur
|
||||
* decomposition, which is the bottleneck.
|
||||
*/
|
||||
RealScalar m_conditionNumber;
|
||||
|
||||
/** \brief Rank of m_A. */
|
||||
Index m_rank;
|
||||
|
||||
/** \brief Rank deficiency of m_A. */
|
||||
Index m_nulls;
|
||||
|
||||
/**
|
||||
* \brief Split p into integral part and fractional part.
|
||||
*
|
||||
* \param[in] p The exponent.
|
||||
* \param[out] p The fractional part ranging in \f$ (-1, 1) \f$.
|
||||
* \param[out] intpart The integral part.
|
||||
*
|
||||
* Only if the fractional part is nonzero, it calls initialize().
|
||||
*/
|
||||
void split(RealScalar& p, RealScalar& intpart);
|
||||
|
||||
/** \brief Perform Schur decomposition for fractional power. */
|
||||
void initialize();
|
||||
|
||||
template<typename ResultType>
|
||||
void computeIntPower(ResultType& res, RealScalar p);
|
||||
|
||||
template<typename ResultType>
|
||||
void computeFracPower(ResultType& res, RealScalar p);
|
||||
|
||||
template<int Rows, int Cols, int Options, int MaxRows, int MaxCols>
|
||||
static void revertSchur(
|
||||
Matrix<ComplexScalar, Rows, Cols, Options, MaxRows, MaxCols>& res,
|
||||
const ComplexMatrix& T,
|
||||
const ComplexMatrix& U);
|
||||
|
||||
template<int Rows, int Cols, int Options, int MaxRows, int MaxCols>
|
||||
static void revertSchur(
|
||||
Matrix<RealScalar, Rows, Cols, Options, MaxRows, MaxCols>& res,
|
||||
const ComplexMatrix& T,
|
||||
const ComplexMatrix& U);
|
||||
};
|
||||
|
||||
template<typename MatrixType>
|
||||
template<typename ResultType>
|
||||
void MatrixPower<MatrixType>::compute(ResultType& res, RealScalar p)
|
||||
{
|
||||
using std::pow;
|
||||
switch (cols()) {
|
||||
case 0:
|
||||
break;
|
||||
case 1:
|
||||
res(0,0) = pow(m_A.coeff(0,0), p);
|
||||
break;
|
||||
default:
|
||||
RealScalar intpart;
|
||||
split(p, intpart);
|
||||
|
||||
res = MatrixType::Identity(rows(), cols());
|
||||
computeIntPower(res, intpart);
|
||||
if (p) computeFracPower(res, p);
|
||||
}
|
||||
}
|
||||
|
||||
template<typename MatrixType>
|
||||
void MatrixPower<MatrixType>::split(RealScalar& p, RealScalar& intpart)
|
||||
{
|
||||
using std::floor;
|
||||
using std::pow;
|
||||
|
||||
intpart = floor(p);
|
||||
p -= intpart;
|
||||
|
||||
// Perform Schur decomposition if it is not yet performed and the power is
|
||||
// not an integer.
|
||||
if (!m_conditionNumber && p)
|
||||
initialize();
|
||||
|
||||
// Choose the more stable of intpart = floor(p) and intpart = ceil(p).
|
||||
if (p > RealScalar(0.5) && p > (1-p) * pow(m_conditionNumber, p)) {
|
||||
--p;
|
||||
++intpart;
|
||||
}
|
||||
}
|
||||
|
||||
template<typename MatrixType>
|
||||
void MatrixPower<MatrixType>::initialize()
|
||||
{
|
||||
const ComplexSchur<MatrixType> schurOfA(m_A);
|
||||
JacobiRotation<ComplexScalar> rot;
|
||||
ComplexScalar eigenvalue;
|
||||
|
||||
m_fT.resizeLike(m_A);
|
||||
m_T = schurOfA.matrixT();
|
||||
m_U = schurOfA.matrixU();
|
||||
m_conditionNumber = m_T.diagonal().array().abs().maxCoeff() / m_T.diagonal().array().abs().minCoeff();
|
||||
|
||||
// Move zero eigenvalues to the bottom right corner.
|
||||
for (Index i = cols()-1; i>=0; --i) {
|
||||
if (m_rank <= 2)
|
||||
return;
|
||||
if (m_T.coeff(i,i) == RealScalar(0)) {
|
||||
for (Index j=i+1; j < m_rank; ++j) {
|
||||
eigenvalue = m_T.coeff(j,j);
|
||||
rot.makeGivens(m_T.coeff(j-1,j), eigenvalue);
|
||||
m_T.applyOnTheRight(j-1, j, rot);
|
||||
m_T.applyOnTheLeft(j-1, j, rot.adjoint());
|
||||
m_T.coeffRef(j-1,j-1) = eigenvalue;
|
||||
m_T.coeffRef(j,j) = RealScalar(0);
|
||||
m_U.applyOnTheRight(j-1, j, rot);
|
||||
}
|
||||
--m_rank;
|
||||
}
|
||||
}
|
||||
|
||||
m_nulls = rows() - m_rank;
|
||||
if (m_nulls) {
|
||||
eigen_assert(m_T.bottomRightCorner(m_nulls, m_nulls).isZero()
|
||||
&& "Base of matrix power should be invertible or with a semisimple zero eigenvalue.");
|
||||
m_fT.bottomRows(m_nulls).fill(RealScalar(0));
|
||||
}
|
||||
}
|
||||
|
||||
template<typename MatrixType>
|
||||
template<typename ResultType>
|
||||
void MatrixPower<MatrixType>::computeIntPower(ResultType& res, RealScalar p)
|
||||
{
|
||||
using std::abs;
|
||||
using std::fmod;
|
||||
RealScalar pp = abs(p);
|
||||
|
||||
if (p<0)
|
||||
m_tmp = m_A.inverse();
|
||||
else
|
||||
m_tmp = m_A;
|
||||
|
||||
while (true) {
|
||||
if (fmod(pp, 2) >= 1)
|
||||
res = m_tmp * res;
|
||||
pp /= 2;
|
||||
if (pp < 1)
|
||||
break;
|
||||
m_tmp *= m_tmp;
|
||||
}
|
||||
}
|
||||
|
||||
template<typename MatrixType>
|
||||
template<typename ResultType>
|
||||
void MatrixPower<MatrixType>::computeFracPower(ResultType& res, RealScalar p)
|
||||
{
|
||||
Block<ComplexMatrix,Dynamic,Dynamic> blockTp(m_fT, 0, 0, m_rank, m_rank);
|
||||
eigen_assert(m_conditionNumber);
|
||||
eigen_assert(m_rank + m_nulls == rows());
|
||||
|
||||
MatrixPowerAtomic<ComplexMatrix>(m_T.topLeftCorner(m_rank, m_rank), p).compute(blockTp);
|
||||
if (m_nulls) {
|
||||
m_fT.topRightCorner(m_rank, m_nulls) = m_T.topLeftCorner(m_rank, m_rank).template triangularView<Upper>()
|
||||
.solve(blockTp * m_T.topRightCorner(m_rank, m_nulls));
|
||||
}
|
||||
revertSchur(m_tmp, m_fT, m_U);
|
||||
res = m_tmp * res;
|
||||
}
|
||||
|
||||
template<typename MatrixType>
|
||||
template<int Rows, int Cols, int Options, int MaxRows, int MaxCols>
|
||||
inline void MatrixPower<MatrixType>::revertSchur(
|
||||
Matrix<ComplexScalar, Rows, Cols, Options, MaxRows, MaxCols>& res,
|
||||
const ComplexMatrix& T,
|
||||
const ComplexMatrix& U)
|
||||
{ res.noalias() = U * (T.template triangularView<Upper>() * U.adjoint()); }
|
||||
|
||||
template<typename MatrixType>
|
||||
template<int Rows, int Cols, int Options, int MaxRows, int MaxCols>
|
||||
inline void MatrixPower<MatrixType>::revertSchur(
|
||||
Matrix<RealScalar, Rows, Cols, Options, MaxRows, MaxCols>& res,
|
||||
const ComplexMatrix& T,
|
||||
const ComplexMatrix& U)
|
||||
{ res.noalias() = (U * (T.template triangularView<Upper>() * U.adjoint())).real(); }
|
||||
|
||||
/**
|
||||
* \ingroup MatrixFunctions_Module
|
||||
*
|
||||
* \brief Proxy for the matrix power of some matrix (expression).
|
||||
*
|
||||
* \tparam Derived type of the base, a matrix (expression).
|
||||
*
|
||||
* This class holds the arguments to the matrix power until it is
|
||||
* assigned or evaluated for some other reason (so the argument
|
||||
* should not be changed in the meantime). It is the return type of
|
||||
* MatrixBase::pow() and related functions and most of the
|
||||
* time this is the only way it is used.
|
||||
*/
|
||||
template<typename Derived>
|
||||
class MatrixPowerReturnValue : public ReturnByValue< MatrixPowerReturnValue<Derived> >
|
||||
{
|
||||
public:
|
||||
typedef typename Derived::PlainObject PlainObject;
|
||||
typedef typename Derived::RealScalar RealScalar;
|
||||
typedef typename Derived::Index Index;
|
||||
|
||||
/**
|
||||
* \brief Constructor.
|
||||
*
|
||||
* \param[in] A %Matrix (expression), the base of the matrix power.
|
||||
* \param[in] p real scalar, the exponent of the matrix power.
|
||||
*/
|
||||
MatrixPowerReturnValue(const Derived& A, RealScalar p) : m_A(A), m_p(p)
|
||||
{ }
|
||||
|
||||
/**
|
||||
* \brief Compute the matrix power.
|
||||
*
|
||||
* \param[out] result \f$ A^p \f$ where \p A and \p p are as in the
|
||||
* constructor.
|
||||
*/
|
||||
template<typename ResultType>
|
||||
inline void evalTo(ResultType& result) const
|
||||
{ MatrixPower<PlainObject>(m_A.eval()).compute(result, m_p); }
|
||||
|
||||
Index rows() const { return m_A.rows(); }
|
||||
Index cols() const { return m_A.cols(); }
|
||||
|
||||
private:
|
||||
const Derived& m_A;
|
||||
const RealScalar m_p;
|
||||
};
|
||||
|
||||
/**
|
||||
* \ingroup MatrixFunctions_Module
|
||||
*
|
||||
* \brief Proxy for the matrix power of some matrix (expression).
|
||||
*
|
||||
* \tparam Derived type of the base, a matrix (expression).
|
||||
*
|
||||
* This class holds the arguments to the matrix power until it is
|
||||
* assigned or evaluated for some other reason (so the argument
|
||||
* should not be changed in the meantime). It is the return type of
|
||||
* MatrixBase::pow() and related functions and most of the
|
||||
* time this is the only way it is used.
|
||||
*/
|
||||
template<typename Derived>
|
||||
class MatrixComplexPowerReturnValue : public ReturnByValue< MatrixComplexPowerReturnValue<Derived> >
|
||||
{
|
||||
public:
|
||||
typedef typename Derived::PlainObject PlainObject;
|
||||
typedef typename std::complex<typename Derived::RealScalar> ComplexScalar;
|
||||
typedef typename Derived::Index Index;
|
||||
|
||||
/**
|
||||
* \brief Constructor.
|
||||
*
|
||||
* \param[in] A %Matrix (expression), the base of the matrix power.
|
||||
* \param[in] p complex scalar, the exponent of the matrix power.
|
||||
*/
|
||||
MatrixComplexPowerReturnValue(const Derived& A, const ComplexScalar& p) : m_A(A), m_p(p)
|
||||
{ }
|
||||
|
||||
/**
|
||||
* \brief Compute the matrix power.
|
||||
*
|
||||
* Because \p p is complex, \f$ A^p \f$ is simply evaluated as \f$
|
||||
* \exp(p \log(A)) \f$.
|
||||
*
|
||||
* \param[out] result \f$ A^p \f$ where \p A and \p p are as in the
|
||||
* constructor.
|
||||
*/
|
||||
template<typename ResultType>
|
||||
inline void evalTo(ResultType& result) const
|
||||
{ result = (m_p * m_A.log()).exp(); }
|
||||
|
||||
Index rows() const { return m_A.rows(); }
|
||||
Index cols() const { return m_A.cols(); }
|
||||
|
||||
private:
|
||||
const Derived& m_A;
|
||||
const ComplexScalar m_p;
|
||||
};
|
||||
|
||||
namespace internal {
|
||||
|
||||
template<typename MatrixPowerType>
|
||||
struct traits< MatrixPowerParenthesesReturnValue<MatrixPowerType> >
|
||||
{ typedef typename MatrixPowerType::PlainObject ReturnType; };
|
||||
|
||||
template<typename Derived>
|
||||
struct traits< MatrixPowerReturnValue<Derived> >
|
||||
{ typedef typename Derived::PlainObject ReturnType; };
|
||||
|
||||
template<typename Derived>
|
||||
struct traits< MatrixComplexPowerReturnValue<Derived> >
|
||||
{ typedef typename Derived::PlainObject ReturnType; };
|
||||
|
||||
}
|
||||
|
||||
template<typename Derived>
|
||||
const MatrixPowerReturnValue<Derived> MatrixBase<Derived>::pow(const RealScalar& p) const
|
||||
{ return MatrixPowerReturnValue<Derived>(derived(), p); }
|
||||
|
||||
template<typename Derived>
|
||||
const MatrixComplexPowerReturnValue<Derived> MatrixBase<Derived>::pow(const std::complex<RealScalar>& p) const
|
||||
{ return MatrixComplexPowerReturnValue<Derived>(derived(), p); }
|
||||
|
||||
} // namespace Eigen
|
||||
|
||||
#endif // EIGEN_MATRIX_POWER
|
||||
@@ -1,368 +0,0 @@
|
||||
// This file is part of Eigen, a lightweight C++ template library
|
||||
// for linear algebra.
|
||||
//
|
||||
// Copyright (C) 2011, 2013 Jitse Niesen <jitse@maths.leeds.ac.uk>
|
||||
//
|
||||
// This Source Code Form is subject to the terms of the Mozilla
|
||||
// Public License v. 2.0. If a copy of the MPL was not distributed
|
||||
// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
|
||||
|
||||
#ifndef EIGEN_MATRIX_SQUARE_ROOT
|
||||
#define EIGEN_MATRIX_SQUARE_ROOT
|
||||
|
||||
namespace Eigen {
|
||||
|
||||
namespace internal {
|
||||
|
||||
// pre: T.block(i,i,2,2) has complex conjugate eigenvalues
|
||||
// post: sqrtT.block(i,i,2,2) is square root of T.block(i,i,2,2)
|
||||
template <typename MatrixType, typename ResultType>
|
||||
void matrix_sqrt_quasi_triangular_2x2_diagonal_block(const MatrixType& T, typename MatrixType::Index i, ResultType& sqrtT)
|
||||
{
|
||||
// TODO: This case (2-by-2 blocks with complex conjugate eigenvalues) is probably hidden somewhere
|
||||
// in EigenSolver. If we expose it, we could call it directly from here.
|
||||
typedef typename traits<MatrixType>::Scalar Scalar;
|
||||
Matrix<Scalar,2,2> block = T.template block<2,2>(i,i);
|
||||
EigenSolver<Matrix<Scalar,2,2> > es(block);
|
||||
sqrtT.template block<2,2>(i,i)
|
||||
= (es.eigenvectors() * es.eigenvalues().cwiseSqrt().asDiagonal() * es.eigenvectors().inverse()).real();
|
||||
}
|
||||
|
||||
// pre: block structure of T is such that (i,j) is a 1x1 block,
|
||||
// all blocks of sqrtT to left of and below (i,j) are correct
|
||||
// post: sqrtT(i,j) has the correct value
|
||||
template <typename MatrixType, typename ResultType>
|
||||
void matrix_sqrt_quasi_triangular_1x1_off_diagonal_block(const MatrixType& T, typename MatrixType::Index i, typename MatrixType::Index j, ResultType& sqrtT)
|
||||
{
|
||||
typedef typename traits<MatrixType>::Scalar Scalar;
|
||||
Scalar tmp = (sqrtT.row(i).segment(i+1,j-i-1) * sqrtT.col(j).segment(i+1,j-i-1)).value();
|
||||
sqrtT.coeffRef(i,j) = (T.coeff(i,j) - tmp) / (sqrtT.coeff(i,i) + sqrtT.coeff(j,j));
|
||||
}
|
||||
|
||||
// similar to compute1x1offDiagonalBlock()
|
||||
template <typename MatrixType, typename ResultType>
|
||||
void matrix_sqrt_quasi_triangular_1x2_off_diagonal_block(const MatrixType& T, typename MatrixType::Index i, typename MatrixType::Index j, ResultType& sqrtT)
|
||||
{
|
||||
typedef typename traits<MatrixType>::Scalar Scalar;
|
||||
Matrix<Scalar,1,2> rhs = T.template block<1,2>(i,j);
|
||||
if (j-i > 1)
|
||||
rhs -= sqrtT.block(i, i+1, 1, j-i-1) * sqrtT.block(i+1, j, j-i-1, 2);
|
||||
Matrix<Scalar,2,2> A = sqrtT.coeff(i,i) * Matrix<Scalar,2,2>::Identity();
|
||||
A += sqrtT.template block<2,2>(j,j).transpose();
|
||||
sqrtT.template block<1,2>(i,j).transpose() = A.fullPivLu().solve(rhs.transpose());
|
||||
}
|
||||
|
||||
// similar to compute1x1offDiagonalBlock()
|
||||
template <typename MatrixType, typename ResultType>
|
||||
void matrix_sqrt_quasi_triangular_2x1_off_diagonal_block(const MatrixType& T, typename MatrixType::Index i, typename MatrixType::Index j, ResultType& sqrtT)
|
||||
{
|
||||
typedef typename traits<MatrixType>::Scalar Scalar;
|
||||
Matrix<Scalar,2,1> rhs = T.template block<2,1>(i,j);
|
||||
if (j-i > 2)
|
||||
rhs -= sqrtT.block(i, i+2, 2, j-i-2) * sqrtT.block(i+2, j, j-i-2, 1);
|
||||
Matrix<Scalar,2,2> A = sqrtT.coeff(j,j) * Matrix<Scalar,2,2>::Identity();
|
||||
A += sqrtT.template block<2,2>(i,i);
|
||||
sqrtT.template block<2,1>(i,j) = A.fullPivLu().solve(rhs);
|
||||
}
|
||||
|
||||
// solves the equation A X + X B = C where all matrices are 2-by-2
|
||||
template <typename MatrixType>
|
||||
void matrix_sqrt_quasi_triangular_solve_auxiliary_equation(MatrixType& X, const MatrixType& A, const MatrixType& B, const MatrixType& C)
|
||||
{
|
||||
typedef typename traits<MatrixType>::Scalar Scalar;
|
||||
Matrix<Scalar,4,4> coeffMatrix = Matrix<Scalar,4,4>::Zero();
|
||||
coeffMatrix.coeffRef(0,0) = A.coeff(0,0) + B.coeff(0,0);
|
||||
coeffMatrix.coeffRef(1,1) = A.coeff(0,0) + B.coeff(1,1);
|
||||
coeffMatrix.coeffRef(2,2) = A.coeff(1,1) + B.coeff(0,0);
|
||||
coeffMatrix.coeffRef(3,3) = A.coeff(1,1) + B.coeff(1,1);
|
||||
coeffMatrix.coeffRef(0,1) = B.coeff(1,0);
|
||||
coeffMatrix.coeffRef(0,2) = A.coeff(0,1);
|
||||
coeffMatrix.coeffRef(1,0) = B.coeff(0,1);
|
||||
coeffMatrix.coeffRef(1,3) = A.coeff(0,1);
|
||||
coeffMatrix.coeffRef(2,0) = A.coeff(1,0);
|
||||
coeffMatrix.coeffRef(2,3) = B.coeff(1,0);
|
||||
coeffMatrix.coeffRef(3,1) = A.coeff(1,0);
|
||||
coeffMatrix.coeffRef(3,2) = B.coeff(0,1);
|
||||
|
||||
Matrix<Scalar,4,1> rhs;
|
||||
rhs.coeffRef(0) = C.coeff(0,0);
|
||||
rhs.coeffRef(1) = C.coeff(0,1);
|
||||
rhs.coeffRef(2) = C.coeff(1,0);
|
||||
rhs.coeffRef(3) = C.coeff(1,1);
|
||||
|
||||
Matrix<Scalar,4,1> result;
|
||||
result = coeffMatrix.fullPivLu().solve(rhs);
|
||||
|
||||
X.coeffRef(0,0) = result.coeff(0);
|
||||
X.coeffRef(0,1) = result.coeff(1);
|
||||
X.coeffRef(1,0) = result.coeff(2);
|
||||
X.coeffRef(1,1) = result.coeff(3);
|
||||
}
|
||||
|
||||
// similar to compute1x1offDiagonalBlock()
|
||||
template <typename MatrixType, typename ResultType>
|
||||
void matrix_sqrt_quasi_triangular_2x2_off_diagonal_block(const MatrixType& T, typename MatrixType::Index i, typename MatrixType::Index j, ResultType& sqrtT)
|
||||
{
|
||||
typedef typename traits<MatrixType>::Scalar Scalar;
|
||||
Matrix<Scalar,2,2> A = sqrtT.template block<2,2>(i,i);
|
||||
Matrix<Scalar,2,2> B = sqrtT.template block<2,2>(j,j);
|
||||
Matrix<Scalar,2,2> C = T.template block<2,2>(i,j);
|
||||
if (j-i > 2)
|
||||
C -= sqrtT.block(i, i+2, 2, j-i-2) * sqrtT.block(i+2, j, j-i-2, 2);
|
||||
Matrix<Scalar,2,2> X;
|
||||
matrix_sqrt_quasi_triangular_solve_auxiliary_equation(X, A, B, C);
|
||||
sqrtT.template block<2,2>(i,j) = X;
|
||||
}
|
||||
|
||||
// pre: T is quasi-upper-triangular and sqrtT is a zero matrix of the same size
|
||||
// post: the diagonal blocks of sqrtT are the square roots of the diagonal blocks of T
|
||||
template <typename MatrixType, typename ResultType>
|
||||
void matrix_sqrt_quasi_triangular_diagonal(const MatrixType& T, ResultType& sqrtT)
|
||||
{
|
||||
using std::sqrt;
|
||||
const Index size = T.rows();
|
||||
for (Index i = 0; i < size; i++) {
|
||||
if (i == size - 1 || T.coeff(i+1, i) == 0) {
|
||||
eigen_assert(T(i,i) >= 0);
|
||||
sqrtT.coeffRef(i,i) = sqrt(T.coeff(i,i));
|
||||
}
|
||||
else {
|
||||
matrix_sqrt_quasi_triangular_2x2_diagonal_block(T, i, sqrtT);
|
||||
++i;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// pre: T is quasi-upper-triangular and diagonal blocks of sqrtT are square root of diagonal blocks of T.
|
||||
// post: sqrtT is the square root of T.
|
||||
template <typename MatrixType, typename ResultType>
|
||||
void matrix_sqrt_quasi_triangular_off_diagonal(const MatrixType& T, ResultType& sqrtT)
|
||||
{
|
||||
const Index size = T.rows();
|
||||
for (Index j = 1; j < size; j++) {
|
||||
if (T.coeff(j, j-1) != 0) // if T(j-1:j, j-1:j) is a 2-by-2 block
|
||||
continue;
|
||||
for (Index i = j-1; i >= 0; i--) {
|
||||
if (i > 0 && T.coeff(i, i-1) != 0) // if T(i-1:i, i-1:i) is a 2-by-2 block
|
||||
continue;
|
||||
bool iBlockIs2x2 = (i < size - 1) && (T.coeff(i+1, i) != 0);
|
||||
bool jBlockIs2x2 = (j < size - 1) && (T.coeff(j+1, j) != 0);
|
||||
if (iBlockIs2x2 && jBlockIs2x2)
|
||||
matrix_sqrt_quasi_triangular_2x2_off_diagonal_block(T, i, j, sqrtT);
|
||||
else if (iBlockIs2x2 && !jBlockIs2x2)
|
||||
matrix_sqrt_quasi_triangular_2x1_off_diagonal_block(T, i, j, sqrtT);
|
||||
else if (!iBlockIs2x2 && jBlockIs2x2)
|
||||
matrix_sqrt_quasi_triangular_1x2_off_diagonal_block(T, i, j, sqrtT);
|
||||
else if (!iBlockIs2x2 && !jBlockIs2x2)
|
||||
matrix_sqrt_quasi_triangular_1x1_off_diagonal_block(T, i, j, sqrtT);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
} // end of namespace internal
|
||||
|
||||
/** \ingroup MatrixFunctions_Module
|
||||
* \brief Compute matrix square root of quasi-triangular matrix.
|
||||
*
|
||||
* \tparam MatrixType type of \p arg, the argument of matrix square root,
|
||||
* expected to be an instantiation of the Matrix class template.
|
||||
* \tparam ResultType type of \p result, where result is to be stored.
|
||||
* \param[in] arg argument of matrix square root.
|
||||
* \param[out] result matrix square root of upper Hessenberg part of \p arg.
|
||||
*
|
||||
* This function computes the square root of the upper quasi-triangular matrix stored in the upper
|
||||
* Hessenberg part of \p arg. Only the upper Hessenberg part of \p result is updated, the rest is
|
||||
* not touched. See MatrixBase::sqrt() for details on how this computation is implemented.
|
||||
*
|
||||
* \sa MatrixSquareRoot, MatrixSquareRootQuasiTriangular
|
||||
*/
|
||||
template <typename MatrixType, typename ResultType>
|
||||
void matrix_sqrt_quasi_triangular(const MatrixType &arg, ResultType &result)
|
||||
{
|
||||
eigen_assert(arg.rows() == arg.cols());
|
||||
result.resize(arg.rows(), arg.cols());
|
||||
internal::matrix_sqrt_quasi_triangular_diagonal(arg, result);
|
||||
internal::matrix_sqrt_quasi_triangular_off_diagonal(arg, result);
|
||||
}
|
||||
|
||||
|
||||
/** \ingroup MatrixFunctions_Module
|
||||
* \brief Compute matrix square root of triangular matrix.
|
||||
*
|
||||
* \tparam MatrixType type of \p arg, the argument of matrix square root,
|
||||
* expected to be an instantiation of the Matrix class template.
|
||||
* \tparam ResultType type of \p result, where result is to be stored.
|
||||
* \param[in] arg argument of matrix square root.
|
||||
* \param[out] result matrix square root of upper triangular part of \p arg.
|
||||
*
|
||||
* Only the upper triangular part (including the diagonal) of \p result is updated, the rest is not
|
||||
* touched. See MatrixBase::sqrt() for details on how this computation is implemented.
|
||||
*
|
||||
* \sa MatrixSquareRoot, MatrixSquareRootQuasiTriangular
|
||||
*/
|
||||
template <typename MatrixType, typename ResultType>
|
||||
void matrix_sqrt_triangular(const MatrixType &arg, ResultType &result)
|
||||
{
|
||||
using std::sqrt;
|
||||
typedef typename MatrixType::Scalar Scalar;
|
||||
|
||||
eigen_assert(arg.rows() == arg.cols());
|
||||
|
||||
// Compute square root of arg and store it in upper triangular part of result
|
||||
// This uses that the square root of triangular matrices can be computed directly.
|
||||
result.resize(arg.rows(), arg.cols());
|
||||
for (Index i = 0; i < arg.rows(); i++) {
|
||||
result.coeffRef(i,i) = sqrt(arg.coeff(i,i));
|
||||
}
|
||||
for (Index j = 1; j < arg.cols(); j++) {
|
||||
for (Index i = j-1; i >= 0; i--) {
|
||||
// if i = j-1, then segment has length 0 so tmp = 0
|
||||
Scalar tmp = (result.row(i).segment(i+1,j-i-1) * result.col(j).segment(i+1,j-i-1)).value();
|
||||
// denominator may be zero if original matrix is singular
|
||||
result.coeffRef(i,j) = (arg.coeff(i,j) - tmp) / (result.coeff(i,i) + result.coeff(j,j));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
namespace internal {
|
||||
|
||||
/** \ingroup MatrixFunctions_Module
|
||||
* \brief Helper struct for computing matrix square roots of general matrices.
|
||||
* \tparam MatrixType type of the argument of the matrix square root,
|
||||
* expected to be an instantiation of the Matrix class template.
|
||||
*
|
||||
* \sa MatrixSquareRootTriangular, MatrixSquareRootQuasiTriangular, MatrixBase::sqrt()
|
||||
*/
|
||||
template <typename MatrixType, int IsComplex = NumTraits<typename internal::traits<MatrixType>::Scalar>::IsComplex>
|
||||
struct matrix_sqrt_compute
|
||||
{
|
||||
/** \brief Compute the matrix square root
|
||||
*
|
||||
* \param[in] arg matrix whose square root is to be computed.
|
||||
* \param[out] result square root of \p arg.
|
||||
*
|
||||
* See MatrixBase::sqrt() for details on how this computation is implemented.
|
||||
*/
|
||||
template <typename ResultType> static void run(const MatrixType &arg, ResultType &result);
|
||||
};
|
||||
|
||||
|
||||
// ********** Partial specialization for real matrices **********
|
||||
|
||||
template <typename MatrixType>
|
||||
struct matrix_sqrt_compute<MatrixType, 0>
|
||||
{
|
||||
typedef typename MatrixType::PlainObject PlainType;
|
||||
template <typename ResultType>
|
||||
static void run(const MatrixType &arg, ResultType &result)
|
||||
{
|
||||
eigen_assert(arg.rows() == arg.cols());
|
||||
|
||||
// Compute Schur decomposition of arg
|
||||
const RealSchur<PlainType> schurOfA(arg);
|
||||
const PlainType& T = schurOfA.matrixT();
|
||||
const PlainType& U = schurOfA.matrixU();
|
||||
|
||||
// Compute square root of T
|
||||
PlainType sqrtT = PlainType::Zero(arg.rows(), arg.cols());
|
||||
matrix_sqrt_quasi_triangular(T, sqrtT);
|
||||
|
||||
// Compute square root of arg
|
||||
result = U * sqrtT * U.adjoint();
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
// ********** Partial specialization for complex matrices **********
|
||||
|
||||
template <typename MatrixType>
|
||||
struct matrix_sqrt_compute<MatrixType, 1>
|
||||
{
|
||||
typedef typename MatrixType::PlainObject PlainType;
|
||||
template <typename ResultType>
|
||||
static void run(const MatrixType &arg, ResultType &result)
|
||||
{
|
||||
eigen_assert(arg.rows() == arg.cols());
|
||||
|
||||
// Compute Schur decomposition of arg
|
||||
const ComplexSchur<PlainType> schurOfA(arg);
|
||||
const PlainType& T = schurOfA.matrixT();
|
||||
const PlainType& U = schurOfA.matrixU();
|
||||
|
||||
// Compute square root of T
|
||||
PlainType sqrtT;
|
||||
matrix_sqrt_triangular(T, sqrtT);
|
||||
|
||||
// Compute square root of arg
|
||||
result = U * (sqrtT.template triangularView<Upper>() * U.adjoint());
|
||||
}
|
||||
};
|
||||
|
||||
} // end namespace internal
|
||||
|
||||
/** \ingroup MatrixFunctions_Module
|
||||
*
|
||||
* \brief Proxy for the matrix square root of some matrix (expression).
|
||||
*
|
||||
* \tparam Derived Type of the argument to the matrix square root.
|
||||
*
|
||||
* This class holds the argument to the matrix square root until it
|
||||
* is assigned or evaluated for some other reason (so the argument
|
||||
* should not be changed in the meantime). It is the return type of
|
||||
* MatrixBase::sqrt() and most of the time this is the only way it is
|
||||
* used.
|
||||
*/
|
||||
template<typename Derived> class MatrixSquareRootReturnValue
|
||||
: public ReturnByValue<MatrixSquareRootReturnValue<Derived> >
|
||||
{
|
||||
protected:
|
||||
typedef typename internal::ref_selector<Derived>::type DerivedNested;
|
||||
|
||||
public:
|
||||
/** \brief Constructor.
|
||||
*
|
||||
* \param[in] src %Matrix (expression) forming the argument of the
|
||||
* matrix square root.
|
||||
*/
|
||||
explicit MatrixSquareRootReturnValue(const Derived& src) : m_src(src) { }
|
||||
|
||||
/** \brief Compute the matrix square root.
|
||||
*
|
||||
* \param[out] result the matrix square root of \p src in the
|
||||
* constructor.
|
||||
*/
|
||||
template <typename ResultType>
|
||||
inline void evalTo(ResultType& result) const
|
||||
{
|
||||
typedef typename internal::nested_eval<Derived, 10>::type DerivedEvalType;
|
||||
typedef typename internal::remove_all<DerivedEvalType>::type DerivedEvalTypeClean;
|
||||
DerivedEvalType tmp(m_src);
|
||||
internal::matrix_sqrt_compute<DerivedEvalTypeClean>::run(tmp, result);
|
||||
}
|
||||
|
||||
Index rows() const { return m_src.rows(); }
|
||||
Index cols() const { return m_src.cols(); }
|
||||
|
||||
protected:
|
||||
const DerivedNested m_src;
|
||||
};
|
||||
|
||||
namespace internal {
|
||||
template<typename Derived>
|
||||
struct traits<MatrixSquareRootReturnValue<Derived> >
|
||||
{
|
||||
typedef typename Derived::PlainObject ReturnType;
|
||||
};
|
||||
}
|
||||
|
||||
template <typename Derived>
|
||||
const MatrixSquareRootReturnValue<Derived> MatrixBase<Derived>::sqrt() const
|
||||
{
|
||||
eigen_assert(rows() == cols());
|
||||
return MatrixSquareRootReturnValue<Derived>(derived());
|
||||
}
|
||||
|
||||
} // end namespace Eigen
|
||||
|
||||
#endif // EIGEN_MATRIX_FUNCTION
|
||||
@@ -1,117 +0,0 @@
|
||||
// This file is part of Eigen, a lightweight C++ template library
|
||||
// for linear algebra.
|
||||
//
|
||||
// Copyright (C) 2010, 2013 Jitse Niesen <jitse@maths.leeds.ac.uk>
|
||||
//
|
||||
// This Source Code Form is subject to the terms of the Mozilla
|
||||
// Public License v. 2.0. If a copy of the MPL was not distributed
|
||||
// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
|
||||
|
||||
#ifndef EIGEN_STEM_FUNCTION
|
||||
#define EIGEN_STEM_FUNCTION
|
||||
|
||||
namespace Eigen {
|
||||
|
||||
namespace internal {
|
||||
|
||||
/** \brief The exponential function (and its derivatives). */
|
||||
template <typename Scalar>
|
||||
Scalar stem_function_exp(Scalar x, int)
|
||||
{
|
||||
using std::exp;
|
||||
return exp(x);
|
||||
}
|
||||
|
||||
/** \brief Cosine (and its derivatives). */
|
||||
template <typename Scalar>
|
||||
Scalar stem_function_cos(Scalar x, int n)
|
||||
{
|
||||
using std::cos;
|
||||
using std::sin;
|
||||
Scalar res;
|
||||
|
||||
switch (n % 4) {
|
||||
case 0:
|
||||
res = std::cos(x);
|
||||
break;
|
||||
case 1:
|
||||
res = -std::sin(x);
|
||||
break;
|
||||
case 2:
|
||||
res = -std::cos(x);
|
||||
break;
|
||||
case 3:
|
||||
res = std::sin(x);
|
||||
break;
|
||||
}
|
||||
return res;
|
||||
}
|
||||
|
||||
/** \brief Sine (and its derivatives). */
|
||||
template <typename Scalar>
|
||||
Scalar stem_function_sin(Scalar x, int n)
|
||||
{
|
||||
using std::cos;
|
||||
using std::sin;
|
||||
Scalar res;
|
||||
|
||||
switch (n % 4) {
|
||||
case 0:
|
||||
res = std::sin(x);
|
||||
break;
|
||||
case 1:
|
||||
res = std::cos(x);
|
||||
break;
|
||||
case 2:
|
||||
res = -std::sin(x);
|
||||
break;
|
||||
case 3:
|
||||
res = -std::cos(x);
|
||||
break;
|
||||
}
|
||||
return res;
|
||||
}
|
||||
|
||||
/** \brief Hyperbolic cosine (and its derivatives). */
|
||||
template <typename Scalar>
|
||||
Scalar stem_function_cosh(Scalar x, int n)
|
||||
{
|
||||
using std::cosh;
|
||||
using std::sinh;
|
||||
Scalar res;
|
||||
|
||||
switch (n % 2) {
|
||||
case 0:
|
||||
res = std::cosh(x);
|
||||
break;
|
||||
case 1:
|
||||
res = std::sinh(x);
|
||||
break;
|
||||
}
|
||||
return res;
|
||||
}
|
||||
|
||||
/** \brief Hyperbolic sine (and its derivatives). */
|
||||
template <typename Scalar>
|
||||
Scalar stem_function_sinh(Scalar x, int n)
|
||||
{
|
||||
using std::cosh;
|
||||
using std::sinh;
|
||||
Scalar res;
|
||||
|
||||
switch (n % 2) {
|
||||
case 0:
|
||||
res = std::sinh(x);
|
||||
break;
|
||||
case 1:
|
||||
res = std::cosh(x);
|
||||
break;
|
||||
}
|
||||
return res;
|
||||
}
|
||||
|
||||
} // end namespace internal
|
||||
|
||||
} // end namespace Eigen
|
||||
|
||||
#endif // EIGEN_STEM_FUNCTION
|
||||
Reference in New Issue
Block a user